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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CSCH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CSCH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic cosecant: \f$1/\sinh(1/x)\f$.

    @par Header <boost/simd/function/csch.hpp>

    @see sinh, tanh, sech, csch, sinhcosh

    @par Example:

      @snippet csch.cpp csch

    @par Possible output:

      @snippet csch.txt csch
  **/
  IEEEValue csch(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/csch.hpp>
#include <boost/simd/function/simd/csch.hpp>

#endif


#include <boost/mpl11/detail/left_folds/variadic.hpp>
#include <boost/mpl11/integer.hpp>

template <typename x, typename y>
using plus = boost::mpl11::integer_c<
    decltype(x::type::value + y::type::value),
    x::type::value + y::type::value
>;

template <typename ...xs>
using sum = boost::mpl11::detail::left_folds::variadic<
    plus, boost::mpl11::int_<0>, xs...
>;

<%= render('_main.erb') %>


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

/*
 Copyright (C) 2007 Marco Bianchetti
 Copyright (C) 2007 François du Vignaud
 Copyright (C) 2007 Giorgio Facchinetti
 Copyright (C) 2012 Ralph Schreyer
 Copyright (C) 2012 Mateusz Kapturski

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

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

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

#include "optimizers.hpp"
#include "utilities.hpp"
#include <boost/make_shared.hpp>
#include <ql/math/optimization/simplex.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/math/optimization/conjugategradient.hpp>
#include <ql/math/optimization/steepestdescent.hpp>
#include <ql/math/optimization/bfgs.hpp>
#include <ql/math/optimization/constraint.hpp>
#include <ql/math/optimization/costfunction.hpp>
#include <ql/math/randomnumbers/mt19937uniformrng.hpp>
#include <ql/math/optimization/differentialevolution.hpp>
#include <ql/math/optimization/goldstein.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

using std::pow;
using std::cos;

namespace {

    struct NamedOptimizationMethod;

    std::vector<boost::shared_ptr<CostFunction> > costFunctions_;
    std::vector<boost::shared_ptr<Constraint> > constraints_;
    std::vector<Array> initialValues_;
    std::vector<Size> maxIterations_, maxStationaryStateIterations_;
    std::vector<Real> rootEpsilons_, functionEpsilons_, gradientNormEpsilons_;
    std::vector<boost::shared_ptr<EndCriteria> > endCriterias_;
    std::vector<std::vector<NamedOptimizationMethod> > optimizationMethods_;
    std::vector<Array> xMinExpected_, yMinExpected_;

    class OneDimensionalPolynomialDegreeN : public CostFunction {
      public:
        OneDimensionalPolynomialDegreeN(const Array& coefficients)
        : coefficients_(coefficients),
          polynomialDegree_(coefficients.size()-1) {}

        Real value(const Array& x) const {
            QL_REQUIRE(x.size()==1,"independent variable must be 1 dimensional");
            Real y = 0;
            for (Size i=0; i<=polynomialDegree_; ++i)
                y += coefficients_[i]*std::pow(x[0],static_cast<int>(i));
            return y;
        }

        Disposable<Array> values(const Array& x) const{
            QL_REQUIRE(x.size()==1,"independent variable must be 1 dimensional");
            Array y(1);
            y[0] = value(x);
            return y;
        }

      private:
        const Array coefficients_;
        const Size polynomialDegree_;
    };

    // The goal of this cost function is simply to call another optimization inside
    // in order to test nested optimizations
    class OptimizationBasedCostFunction : public CostFunction {
      public:
        Real value(const Array&) const { return 1.0; }

        Disposable<Array> values(const Array&) const{
            // dummy nested optimization
            Array coefficients(3, 1.0);
            OneDimensionalPolynomialDegreeN oneDimensionalPolynomialDegreeN(coefficients);
            NoConstraint constraint;
            Array initialValues(1, 100.0);
            Problem problem(oneDimensionalPolynomialDegreeN, constraint,
                            initialValues);
            LevenbergMarquardt optimizationMethod;
            //Simplex optimizationMethod(0.1);
            //ConjugateGradient optimizationMethod;
            //SteepestDescent optimizationMethod;
            EndCriteria endCriteria(1000, 100, 1e-5, 1e-5, 1e-5);
            optimizationMethod.minimize(problem, endCriteria);
            // return dummy result
            Array dummy(1,0);
            return dummy;
        }
    };

    enum OptimizationMethodType {simplex,
                                 levenbergMarquardt,
                                 levenbergMarquardt2,
                                 conjugateGradient,
                                 conjugateGradient_goldstein,
                                 steepestDescent,
                                 steepestDescent_goldstein,
                                 bfgs,
                                 bfgs_goldstein};

    std::string optimizationMethodTypeToString(OptimizationMethodType type) {
        switch (type) {
          case simplex:
            return "Simplex";
          case levenbergMarquardt:
            return "Levenberg Marquardt";
          case levenbergMarquardt2:
            return "Levenberg Marquardt (cost function's jacbobian)";
          case conjugateGradient:
            return "Conjugate Gradient";
          case steepestDescent:
            return "Steepest Descent";
          case bfgs:
            return "BFGS";
          case conjugateGradient_goldstein:
              return "Conjugate Gradient (Goldstein line search)";
          case steepestDescent_goldstein:
              return "Steepest Descent (Goldstein line search)";
          case bfgs_goldstein:
              return "BFGS (Goldstein line search)";
          default:
            QL_FAIL("unknown OptimizationMethod type");
        }
    }

    struct NamedOptimizationMethod {
        boost::shared_ptr<OptimizationMethod> optimizationMethod;
        std::string name;
    };

    boost::shared_ptr<OptimizationMethod> makeOptimizationMethod(
                                OptimizationMethodType optimizationMethodType,
                                Real simplexLambda,
                                Real levenbergMarquardtEpsfcn,
                                Real levenbergMarquardtXtol,
                                Real levenbergMarquardtGtol) {
        switch (optimizationMethodType) {
          case simplex:
            return boost::shared_ptr<OptimizationMethod>(
                new Simplex(simplexLambda));
          case levenbergMarquardt:
            return boost::shared_ptr<OptimizationMethod>(
                new LevenbergMarquardt(levenbergMarquardtEpsfcn,
                                       levenbergMarquardtXtol,
                                       levenbergMarquardtGtol));
          case levenbergMarquardt2:
            return boost::shared_ptr<OptimizationMethod>(
                new LevenbergMarquardt(levenbergMarquardtEpsfcn,
                                       levenbergMarquardtXtol,
                                       levenbergMarquardtGtol,
                                       true));
          case conjugateGradient:
            return boost::shared_ptr<OptimizationMethod>(new ConjugateGradient);
          case steepestDescent:
            return boost::shared_ptr<OptimizationMethod>(new SteepestDescent);
          case bfgs:
            return boost::shared_ptr<OptimizationMethod>(new BFGS);
          case conjugateGradient_goldstein:
              return boost::shared_ptr<OptimizationMethod>(new ConjugateGradient(boost::make_shared<GoldsteinLineSearch>()));
          case steepestDescent_goldstein:
              return boost::shared_ptr<OptimizationMethod>(new SteepestDescent(boost::make_shared<GoldsteinLineSearch>()));
          case bfgs_goldstein:
              return boost::shared_ptr<OptimizationMethod>(new BFGS(boost::make_shared<GoldsteinLineSearch>()));
          default:
            QL_FAIL("unknown OptimizationMethod type");
        }
    }

    std::vector<NamedOptimizationMethod> makeOptimizationMethods(
                             OptimizationMethodType optimizationMethodTypes[],
                             Size optimizationMethodNb,
                             Real simplexLambda,
                             Real levenbergMarquardtEpsfcn,
                             Real levenbergMarquardtXtol,
                             Real levenbergMarquardtGtol) {
        std::vector<NamedOptimizationMethod> results;
        for (Size i=0; i<optimizationMethodNb; ++i) {
            NamedOptimizationMethod namedOptimizationMethod;
            namedOptimizationMethod.optimizationMethod = makeOptimizationMethod(
                optimizationMethodTypes[i],
                simplexLambda,
                levenbergMarquardtEpsfcn,
                levenbergMarquardtXtol,
                levenbergMarquardtGtol);
            namedOptimizationMethod.name
                = optimizationMethodTypeToString(optimizationMethodTypes[i]);
            results.push_back(namedOptimizationMethod);
        }
        return results;
    }

    Real maxDifference(const Array& a, const Array& b) {
        Array diff = a-b;
        Real maxDiff = 0.0;
        for (Size i=0; i<diff.size(); ++i)
            maxDiff = std::max(maxDiff, std::fabs(diff[i]));
        return maxDiff;
    }

    // Set up, for each cost function, all the ingredients for optimization:
    // constraint, initial guess, end criteria, optimization methods.
    void setup() {

        // Cost function n. 1: 1D polynomial of degree 2 (parabolic function y=a*x^2+b*x+c)
        const Real a = 1;   // required a > 0
        const Real b = 1;
        const Real c = 1;
        Array coefficients(3);
        coefficients[0]= c;
        coefficients[1]= b;
        coefficients[2]= a;
        costFunctions_.push_back(boost::shared_ptr<CostFunction>(
            new OneDimensionalPolynomialDegreeN(coefficients)));
        // Set constraint for optimizers: unconstrained problem
        constraints_.push_back(boost::shared_ptr<Constraint>(new NoConstraint()));
        // Set initial guess for optimizer
        Array initialValue(1);
        initialValue[0] = -100;
        initialValues_.push_back(initialValue);
        // Set end criteria for optimizer
        maxIterations_.push_back(10000);                // maxIterations
        maxStationaryStateIterations_.push_back(100);   // MaxStationaryStateIterations
        rootEpsilons_.push_back(1e-8);                  // rootEpsilon
        functionEpsilons_.push_back(1e-8);              // functionEpsilon
        gradientNormEpsilons_.push_back(1e-8);          // gradientNormEpsilon
        endCriterias_.push_back(boost::shared_ptr<EndCriteria>(
            new EndCriteria(maxIterations_.back(), maxStationaryStateIterations_.back(),
                            rootEpsilons_.back(), functionEpsilons_.back(),
                            gradientNormEpsilons_.back())));
        // Set optimization methods for optimizer
        OptimizationMethodType optimizationMethodTypes[] = {
            simplex, levenbergMarquardt, levenbergMarquardt2, conjugateGradient,
            bfgs //, steepestDescent
        };
        Real simplexLambda = 0.1;                   // characteristic search length for simplex
        Real levenbergMarquardtEpsfcn = 1.0e-8;     // parameters specific for Levenberg-Marquardt
        Real levenbergMarquardtXtol   = 1.0e-8;     //
        Real levenbergMarquardtGtol   = 1.0e-8;     //
        optimizationMethods_.push_back(makeOptimizationMethods(
            optimizationMethodTypes, LENGTH(optimizationMethodTypes),
            simplexLambda, levenbergMarquardtEpsfcn, levenbergMarquardtXtol,
            levenbergMarquardtGtol));
        // Set expected results for optimizer
        Array xMinExpected(1),yMinExpected(1);
        xMinExpected[0] = -b/(2.0*a);
        yMinExpected[0] = -(b*b-4.0*a*c)/(4.0*a);
        xMinExpected_.push_back(xMinExpected);
        yMinExpected_.push_back(yMinExpected);
    }

}

void OptimizersTest::test() {
    BOOST_TEST_MESSAGE("Testing optimizers...");

    setup();

    // Loop over problems (currently there is only 1 problem)
    for (Size i=0; i<costFunctions_.size(); ++i) {
        Problem problem(*costFunctions_[i], *constraints_[i],
                        initialValues_[i]);
        Array initialValues = problem.currentValue();
        // Loop over optimizers
        for (Size j=0; j<(optimizationMethods_[i]).size(); ++j) {
            Real rootEpsilon = endCriterias_[i]->rootEpsilon();
            Size endCriteriaTests = 1;
           // Loop over rootEpsilon
            for (Size k=0; k<endCriteriaTests; ++k) {
                problem.setCurrentValue(initialValues);
                EndCriteria endCriteria(
                            endCriterias_[i]->maxIterations(),
                            endCriterias_[i]->maxStationaryStateIterations(),
                            rootEpsilon,
                            endCriterias_[i]->functionEpsilon(),
                            endCriterias_[i]->gradientNormEpsilon());
                rootEpsilon *= .1;
                EndCriteria::Type endCriteriaResult =
                    optimizationMethods_[i][j].optimizationMethod->minimize(
                    problem, endCriteria);
                Array xMinCalculated = problem.currentValue();
                Array yMinCalculated = problem.values(xMinCalculated);

                // Check optimization results vs known solution
                bool completed;
                switch (endCriteriaResult) {
                  case EndCriteria::None:
                  case EndCriteria::MaxIterations:
                  case EndCriteria::Unknown:
                    completed = false;
                    break;
                  default:
                    completed = true;
                }

                Real xError = maxDifference(xMinCalculated,xMinExpected_[i]);
                Real yError = maxDifference(yMinCalculated,yMinExpected_[i]);

                bool correct = (xError <= endCriteria.rootEpsilon() ||
                                yError <= endCriteria.functionEpsilon());

                if ((!completed) || (!correct))
                    BOOST_ERROR("costFunction # = " << i <<
                                "\nOptimizer: " <<
                                optimizationMethods_[i][j].name <<
                                "\n    function evaluations: " <<
                                problem.functionEvaluation()  <<
                                "\n    gradient evaluations: " <<
                                problem.gradientEvaluation() <<
                                "\n    x expected:           " <<
                                xMinExpected_[i] <<
                                "\n    x calculated:         " <<
                                std::setprecision(9) << xMinCalculated <<
                                "\n    x difference:         " <<
                                xMinExpected_[i]- xMinCalculated <<
                                "\n    rootEpsilon:          " <<
                                std::setprecision(9) <<
                                endCriteria.rootEpsilon() <<
                                "\n    y expected:           " <<
                                yMinExpected_[i] <<
                                "\n    y calculated:         " <<
                                std::setprecision(9) << yMinCalculated <<
                                "\n    y difference:         " <<
                                yMinExpected_[i]- yMinCalculated <<
                                "\n    functionEpsilon:      " <<
                                std::setprecision(9) <<
                                endCriteria.functionEpsilon() <<
                                "\n    endCriteriaResult:    " <<
                                endCriteriaResult);
            }
        }
    }
}

void OptimizersTest::nestedOptimizationTest() {
    BOOST_TEST_MESSAGE("Testing nested optimizations...");
    OptimizationBasedCostFunction optimizationBasedCostFunction;
    NoConstraint constraint;
    Array initialValues(1, 0.0);
    Problem problem(optimizationBasedCostFunction, constraint,
                    initialValues);
    LevenbergMarquardt optimizationMethod;
    //Simplex optimizationMethod(0.1);
    //ConjugateGradient optimizationMethod;
    //SteepestDescent optimizationMethod;
    EndCriteria endCriteria(1000, 100, 1e-5, 1e-5, 1e-5);
    optimizationMethod.minimize(problem, endCriteria);

}

namespace {

    class FirstDeJong : public CostFunction {
      public:
        Disposable<Array> values(const Array& x) const {
            Array retVal(x.size(),value(x));
            return retVal;
        }
        Real value(const Array& x) const {
            return DotProduct(x,x);
        }
    };

    class SecondDeJong : public CostFunction {
      public:
        Disposable<Array> values(const Array& x) const {
            Array retVal(x.size(),value(x));
            return retVal;
        }
        Real value(const Array& x) const {
            return  100.0*(x[0]*x[0]-x[1])*(x[0]*x[0]-x[1])
                  + (1.0-x[0])*(1.0-x[0]);
        }
    };

    class ModThirdDeJong : public CostFunction {
      public:
        Disposable<Array> values(const Array& x) const {
            Array retVal(x.size(),value(x));
            return retVal;
        }
        Real value(const Array& x) const {
            Real fx = 0.0;
            for (Size i=0; i<x.size(); ++i) {
                fx += std::floor(x[i])*std::floor(x[i]);
            }
            return fx;
        }
    };

    class ModFourthDeJong : public CostFunction {
      public:
        ModFourthDeJong()
        : uniformRng_(MersenneTwisterUniformRng(4711)) {
        }
        Disposable<Array> values(const Array& x) const {
            Array retVal(x.size(),value(x));
            return retVal;
        }
        Real value(const Array& x) const {
            Real fx = 0.0;
            for (Size i=0; i<x.size(); ++i) {
                fx += (i+1.0)*pow(x[i],4.0) + uniformRng_.nextReal();
            }
            return fx;
        }
        MersenneTwisterUniformRng uniformRng_;
    };

    class Griewangk : public CostFunction {
      public:
        Disposable<Array> values(const Array& x) const{
            Array retVal(x.size(),value(x));
            return retVal;
        }
        Real value(const Array& x) const {
            Real fx = 0.0;
            for (Size i=0; i<x.size(); ++i) {
                fx += x[i]*x[i]/4000.0;
            }
            Real p = 1.0;
            for (Size i=0; i<x.size(); ++i) {
                p *= cos(x[i]/sqrt(i+1.0));
            }
            return fx - p + 1.0;
        }
    };
}

void OptimizersTest::testDifferentialEvolution() {
    BOOST_TEST_MESSAGE("Testing differential evolution...");

    /* Note:
    *
    * The "ModFourthDeJong" doesn't have a well defined optimum because
    * of its noisy part. It just has to be <= 15 in our example.
    * The concrete value might differ for a different input and
    * different random numbers.
    *
    * The "Griewangk" function is an example where the adaptive
    * version of DifferentialEvolution turns out to be more successful.
    */

    DifferentialEvolution::Configuration conf =
        DifferentialEvolution::Configuration()
        .withStepsizeWeight(0.4)
        .withBounds()
        .withCrossoverProbability(0.35)
        .withPopulationMembers(500)
        .withStrategy(DifferentialEvolution::BestMemberWithJitter)
        .withCrossoverType(DifferentialEvolution::Normal)
        .withAdaptiveCrossover()
        .withSeed(3242);
    DifferentialEvolution deOptim(conf);

    DifferentialEvolution::Configuration conf2 =
        DifferentialEvolution::Configuration()
        .withStepsizeWeight(1.8)
        .withBounds()
        .withCrossoverProbability(0.9)
        .withPopulationMembers(1000)
        .withStrategy(DifferentialEvolution::Rand1SelfadaptiveWithRotation)
        .withCrossoverType(DifferentialEvolution::Normal)
        .withAdaptiveCrossover()
        .withSeed(3242);
    DifferentialEvolution deOptim2(conf2);

    std::vector<DifferentialEvolution > diffEvolOptimisers;
    diffEvolOptimisers.push_back(deOptim);
    diffEvolOptimisers.push_back(deOptim);
    diffEvolOptimisers.push_back(deOptim);
    diffEvolOptimisers.push_back(deOptim);
    diffEvolOptimisers.push_back(deOptim2);

    std::vector<boost::shared_ptr<CostFunction> > costFunctions;
    costFunctions.push_back(boost::shared_ptr<CostFunction>(new FirstDeJong));
    costFunctions.push_back(boost::shared_ptr<CostFunction>(new SecondDeJong));
    costFunctions.push_back(boost::shared_ptr<CostFunction>(new ModThirdDeJong));
    costFunctions.push_back(boost::shared_ptr<CostFunction>(new ModFourthDeJong));
    costFunctions.push_back(boost::shared_ptr<CostFunction>(new Griewangk));

    std::vector<BoundaryConstraint> constraints;
    constraints.push_back(BoundaryConstraint(-10.0, 10.0));
    constraints.push_back(BoundaryConstraint(-10.0, 10.0));
    constraints.push_back(BoundaryConstraint(-10.0, 10.0));
    constraints.push_back(BoundaryConstraint(-10.0, 10.0));
    constraints.push_back(BoundaryConstraint(-600.0, 600.0));

    std::vector<Array> initialValues;
    initialValues.push_back(Array(3, 5.0));
    initialValues.push_back(Array(2, 5.0));
    initialValues.push_back(Array(5, 5.0));
    initialValues.push_back(Array(30, 5.0));
    initialValues.push_back(Array(10, 100.0));

    std::vector<EndCriteria> endCriteria;
    endCriteria.push_back(EndCriteria(100, 10, 1e-10, 1e-8, Null<Real>()));
    endCriteria.push_back(EndCriteria(100, 10, 1e-10, 1e-8, Null<Real>()));
    endCriteria.push_back(EndCriteria(100, 10, 1e-10, 1e-8, Null<Real>()));
    endCriteria.push_back(EndCriteria(500, 100, 1e-10, 1e-8, Null<Real>()));
    endCriteria.push_back(EndCriteria(1000, 800, 1e-12, 1e-10, Null<Real>()));

    std::vector<Real> minima;
    minima.push_back(0.0);
    minima.push_back(0.0);
    minima.push_back(0.0);
    minima.push_back(10.9639796558);
    minima.push_back(0.0);

    for (Size i = 0; i < costFunctions.size(); ++i) {
        Problem problem(*costFunctions[i], constraints[i], initialValues[i]);
        diffEvolOptimisers[i].minimize(problem, endCriteria[i]);

        if (i != 3) {
            // stable
            if (std::fabs(problem.functionValue() - minima[i]) > 1e-8) {
                BOOST_ERROR("costFunction # " << i
                            << "\ncalculated: " << problem.functionValue()
                            << "\nexpected:   " << minima[i]);
            }
        } else {
            // this case is unstable due to randomness; we're good as
            // long as the result is below 15
            if (problem.functionValue() > 15) {
                BOOST_ERROR("costFunction # " << i
                            << "\ncalculated: " << problem.functionValue()
                            << "\nexpected:   " << "less than 15");
            }
        }
    }
}

test_suite* OptimizersTest::suite() {
    test_suite* suite = BOOST_TEST_SUITE("Optimizers tests");
    suite->add(QUANTLIB_TEST_CASE(&OptimizersTest::test));
    suite->add(QUANTLIB_TEST_CASE(&OptimizersTest::nestedOptimizationTest));
    suite->add(QUANTLIB_TEST_CASE(&OptimizersTest::testDifferentialEvolution));
    return suite;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/kinematics/skeleton/skeleton_types.h>
#include <OpenTissue/kinematics/inverse/inverse.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

typedef OpenTissue::math::default_math_types		  			                 math_types;
typedef math_types::vector3_type                                         vector3_type;
typedef math_types::value_traits                                         value_traits;
typedef math_types::real_type                                            real_type;
typedef OpenTissue::skeleton::DefaultBoneTraits<math_types>              base_bone_traits;
typedef OpenTissue::kinematics::inverse::BoneTraits<base_bone_traits>    bone_traits;
typedef OpenTissue::skeleton::Types<math_types, bone_traits>             skeleton_types;
typedef skeleton_types::skeleton_type                                    skeleton_type;
typedef skeleton_types::bone_type                                        bone_type;

BOOST_AUTO_TEST_SUITE(opentissue_kinematics_inverse_set_joint_parameters);

BOOST_AUTO_TEST_CASE(test_cases)
{
  using std::fabs;

  real_type const tol      = boost::numeric_cast<real_type>(0.1);

  // Create some skeleton to test with
  skeleton_type skeleton;

  bone_type * b0 = skeleton.create_bone();
  bone_type * b1 = skeleton.create_bone(b0);
  bone_type * b2 = skeleton.create_bone(b1);

  // Skeleton hierarchy looks like this
  //
  //            |  hinge
  //            b0
  //    slider /
  //          b1
  //    ball  |
  //          b2
  //
  vector3_type const hinge_axis   = vector3_type(value_traits::zero(), value_traits::zero(), value_traits::one() );
  real_type    const hinge_angle  = value_traits::pi_half();
  vector3_type const slider_axis  = vector3_type(value_traits::one(), value_traits::zero(), value_traits::zero() );
  real_type    const slider_value = value_traits::four();

  real_type    const phi          = value_traits::pi_quarter();
  real_type    const psi          = value_traits::pi_half();
  real_type    const theta        = value_traits::pi_half();

  b0->type() = bone_traits::hinge_type;
  b0->bind_pose().T() = vector3_type(value_traits::zero(), value_traits::one(), value_traits::zero() );
  b0->bind_pose().Q().Ru( hinge_angle, hinge_axis );

  b1->type() = bone_traits::slider_type;
  b1->bind_pose().T() = slider_axis*slider_value;
  b1->bind_pose().Q().identity();

  b2->type() = bone_traits::ball_type;
  b2->bind_pose().T().clear();
  b2->bind_pose().Q() = OpenTissue::math::Rz(phi)*OpenTissue::math::Ry(psi)*OpenTissue::math::Rz(theta);

  OpenTissue::kinematics::inverse::set_joint_parameters( skeleton );

  BOOST_CHECK( b0->type() == bone_traits::hinge_type );
  BOOST_CHECK( b1->type() == bone_traits::slider_type );
  BOOST_CHECK( b2->type() == bone_traits::ball_type );
  BOOST_CHECK_CLOSE( b0->u()(0), hinge_axis(0), tol);
  BOOST_CHECK_CLOSE( b0->u()(1), hinge_axis(1), tol);
  BOOST_CHECK_CLOSE( b0->u()(2), hinge_axis(2), tol);

  BOOST_CHECK_CLOSE( OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta(*b0, 0), hinge_angle, tol);
  BOOST_CHECK_CLOSE( b1->u()(0), slider_axis(0), tol);
  BOOST_CHECK_CLOSE( b1->u()(1), slider_axis(1), tol);
  BOOST_CHECK_CLOSE( b1->u()(2), slider_axis(2), tol);
  BOOST_CHECK_CLOSE( OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta(*b1, 0), slider_value, tol);
  BOOST_CHECK_CLOSE( OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta(*b2, 0), phi, tol );
  BOOST_CHECK_CLOSE( OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta(*b2, 1), psi, tol );
  BOOST_CHECK_CLOSE( OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta(*b2, 2), theta, tol );
}

BOOST_AUTO_TEST_SUITE_END();


#include<stdio.h>
#include"mex.h"

#include "EdgeSE3ProjectDirectWithDirstortG2oLM.cpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

// 李群李代数 库
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace g2o;

// 一次测量的值，包括一个世界坐标系下三维点,以及投影的对应的图像
struct Measurement
{
    Measurement ( Eigen::Vector3d p, float * im) : pos_world (p), image(im){}
    Eigen::Vector3d pos_world;
    float* image;
};

bool calibrationEstimationDirectG2OLM ( const vector<Measurement>& measurements, double *D, double *K, double *imageSize, Eigen::Isometry3d& Tcw, bool verbose, int max_iter)
{
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( verbose );

    // 添加顶点
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        EdgeSE3ProjectDirectWithDirstortG2oLM* edge = new EdgeSE3ProjectDirectWithDirstortG2oLM (
            m.pos_world,
            D, K, imageSize, m.image
        );
        edge->setVertex (0, pose );
        edge->setMeasurement (0);
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    // cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    mexPrintf("edges in graph: %d\n", optimizer.edges().size());
    optimizer.initializeOptimization();
    optimizer.optimize ( max_iter );
    Tcw = pose->estimate();
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    // nlhs represent the number of parameters of the output
    // plhs is a array of the mxarray pointers, each pointing to the output
    // nrhs represents the number of parameters of the input
    // prhs is a array of the mxarray pointers, each pointing to the input

    // prhs[0], 6x1 matrix
    // prhs[1], Mx1 cell, each cell with NX3 points
    // prhs[2], Mx1 cell, each cell with PXQ single matrix
    // prhs[3], 1x2, or 1x3, or 1x4, or 1x5 matrix
    // prhs[4], 3x3 matrix
    // prhs[5], 1x2 matrix
    // prhs[6], 1x1 matrix
    // prhs[7], 1x1 matrix

    if(nrhs < 8){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidNumInputs", "at least 8 input arguments required");
        return;
    }

    // get the init xi
    const size_t *dimArrayOfInitXi = mxGetDimensions(prhs[0]);
    size_t sizeRowsInitXi = *(dimArrayOfInitXi + 0);
    size_t sizeColsInitXi = *(dimArrayOfInitXi + 1);
    if(sizeRowsInitXi != 6 || sizeColsInitXi != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 1st param should be 6x1");
        return;
    }
    double *ptrInitXi = (double *)(mxGetPr(prhs[0]));
    Eigen::Matrix<double, 6, 1> initXi;
    for(int i = 0; i < 6; i++){
        initXi(i, 0) = *(ptrInitXi + i);
    }
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the chessboard lidar points
    const size_t *dimArrayOfChessboardLidarPoints = mxGetDimensions(prhs[1]);
    size_t sizeRowsCellChessboardLidarPoints = *(dimArrayOfChessboardLidarPoints + 0);
    size_t sizeColsCellChessboardLidarPoints = *(dimArrayOfChessboardLidarPoints + 1);
    if(sizeColsCellChessboardLidarPoints != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 2st param should be Mx1 cell");
        return;
    }
    std::vector<std::vector<Eigen::Vector3d>>chessboardLidarPoints;
    size_t total_measurements = 0;
    for(int i = 0; i < sizeRowsCellChessboardLidarPoints; i++){
        // get the dimensions of each cell
        mxArray *cur_mArray_chessboardLidarPoints = mxGetCell(prhs[1], i);
        const size_t *dimArrayOfCurrentChessboardLidarPoints = mxGetDimensions(cur_mArray_chessboardLidarPoints);
        size_t sizeRowsCurrentChessboardLidarPoints = *(dimArrayOfCurrentChessboardLidarPoints + 0);
        size_t sizeColsCurrentChessboardLidarPoints = *(dimArrayOfCurrentChessboardLidarPoints + 1);
        if(sizeColsCurrentChessboardLidarPoints != 3){
            mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 2st param should be cell with Px3");
            return;
        }
        float *cur_ptr_chessboardLidarPoints = (float *)(mxGetPr(cur_mArray_chessboardLidarPoints));
        std::vector<Eigen::Vector3d>currentCellChessboardLidarPoints;
        total_measurements = total_measurements + sizeRowsCurrentChessboardLidarPoints;
        for(int j = 0; j < sizeRowsCurrentChessboardLidarPoints; j++){
            Eigen::Vector3d current_chessboardLidarPoint;
            current_chessboardLidarPoint[0] = cur_ptr_chessboardLidarPoints[0 * sizeRowsCurrentChessboardLidarPoints + j];
            current_chessboardLidarPoint[1] = cur_ptr_chessboardLidarPoints[1 * sizeRowsCurrentChessboardLidarPoints + j];
            current_chessboardLidarPoint[2] = cur_ptr_chessboardLidarPoints[2 * sizeRowsCurrentChessboardLidarPoints + j];
            currentCellChessboardLidarPoints.push_back(current_chessboardLidarPoint);
        }
        chessboardLidarPoints.push_back(currentCellChessboardLidarPoints);
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the chessoard_Dt_Mask
    const size_t *dimArrayOfChessboardMaskDT = mxGetDimensions(prhs[2]);
    size_t sizeRowsCellChessboardMaskDT = *(dimArrayOfChessboardMaskDT + 0);
    size_t sizeColsCellChessboardMaskDT = *(dimArrayOfChessboardMaskDT + 1);
    if(sizeColsCellChessboardMaskDT != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 3st param should be Mx1 cell");
        return;
    }
    std::vector<float *>chessboardMaskDTPtr;
    for(int i = 0; i < sizeRowsCellChessboardMaskDT; i++){
        // get the dimensions of each cell
        mxArray *cur_mArray_chessboardMaskDT = mxGetCell(prhs[2], i);

        float *cur_ptr_chessboardMaskDT = (float *)(mxGetPr(cur_mArray_chessboardMaskDT));
        chessboardMaskDTPtr.push_back(cur_ptr_chessboardMaskDT);
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the distortion parameters
    const size_t *dimArrayOfD = mxGetDimensions(prhs[3]);
    size_t sizeRowsD = *(dimArrayOfD + 0);
    size_t sizeColsD = *(dimArrayOfD + 1);
    if(sizeRowsD != 1 || sizeColsD != 5){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 4st param should be 1x5 array");
        return;
    }
    double *ptrD = (double *)(mxGetPr(prhs[3]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the intrinsic parameters K
    const size_t *dimArrayOfK = mxGetDimensions(prhs[4]);
    size_t sizeRowsK = *(dimArrayOfK + 0);
    size_t sizeColsK = *(dimArrayOfK + 1);
    if(sizeRowsK != 3 || sizeColsK != 3){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 5st param should be 3X3 array");
        return;
    }
    double *ptrK = (double *)(mxGetPr(prhs[4]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the imageSize
    const size_t *dimArrayOfImageSize = mxGetDimensions(prhs[5]);
    size_t sizeRowsImageSize = *(dimArrayOfImageSize + 0);
    size_t sizeColsImageSize = *(dimArrayOfImageSize + 1);
    if(sizeRowsImageSize != 1 || sizeColsImageSize != 2){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 6st param should be 1x2 array");
        return;
    }
    double *ptrImageSize = (double *)(mxGetPr(prhs[5]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the verbose params
    const size_t *dimArrayOfVerbose = mxGetDimensions(prhs[6]);
    size_t sizeRowsVerbose= *(dimArrayOfVerbose + 0);
    size_t sizeColsVerbose = *(dimArrayOfVerbose + 1);
    if(sizeRowsVerbose != 1 || sizeColsVerbose != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 7st param should be 1x1 array");
        return;
    }
    double *ptrVerbose = (double *)(mxGetPr(prhs[6]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the maxIter params
    const size_t *dimArrayOfMaxIter = mxGetDimensions(prhs[6]);
    size_t sizeRowsMaxIter = *(dimArrayOfMaxIter + 0);
    size_t sizeColsMaxIter = *(dimArrayOfMaxIter + 1);
    if(sizeRowsMaxIter != 1 || sizeColsMaxIter != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 8st param should be 1x1 array");
        return;
    }
    double *ptrMaxIter = (double *)(mxGetPr(prhs[7]));

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    std::vector<Measurement> measurements;
    std::vector<bool> isInners;
    Eigen::Isometry3d Tlc = Eigen::Isometry3d::Identity();

    // get the init tcl from the init Xi
    Sophus::SE3<double> SE3 = Sophus::SE3<double>::exp(initXi);
    Eigen::Matrix<double, 4, 4> SE3Matrix = SE3.matrix();
    Tlc.matrix() << SE3Matrix;

    for(int i = 0; i < chessboardMaskDTPtr.size(); i++){
        float *current_chessboardMaskDTPtr = chessboardMaskDTPtr[i];
        std::vector<Eigen::Vector3d> current_chessboardLidarPoints = chessboardLidarPoints[i];
        for(int j = 0; j < current_chessboardLidarPoints.size(); j++){
            measurements.push_back(Measurement(current_chessboardLidarPoints[j], current_chessboardMaskDTPtr));
        }
    }
    isInners.resize(measurements.size());

    calibrationEstimationDirectG2OLM ( measurements, ptrD, ptrK, ptrImageSize, Tlc, bool(ptrVerbose[0]), int(ptrMaxIter[0]));

    Sophus::SE3<double> SE3_Rt(Tlc.rotation(), Tlc.translation());

    // cout<<"SE3 = "<<endl<<SE3_Rt.matrix()<<endl;

    Eigen::Matrix<double,6,1> se3_out = SE3_Rt.log();

    // the output xi will be 1x6
    size_t dimArrayOfXi[2] = { 1, 6 };
    plhs[0] = mxCreateNumericArray(2, dimArrayOfXi, mxDOUBLE_CLASS, mxREAL);
    double *out_xi = (double *)mxGetData(plhs[0]);

    for(int i = 0; i < 6; i++){
        out_xi[i] = se3_out(i,0);
    }
}


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


#define _USE_MATH_DEFINES
#include <iostream>
#include <memory>
#include <cmath>
#include <vector>
#include <stdlib.h>
#include <ctype.h>
#include <string.h>

#include "Misc.h"
#include "Crc32.h"
using namespace crc32_constexpr;

namespace {
    std::random_device rnd;
    std::mt19937 mt(rnd());

    double rand(double min, double max) {
        if (min > max) {
            const double t = min;
            min = max;
            max = t;
        }
        const double r = SU_TO_DOUBLE(mt()) / SU_TO_DOUBLE(mt.max());
        const double d = max - min;
        return min + r * d;
    }
}

#include "MoverFunctionExpression.h"

#define SU_DEF_VARIABLE_MOVER_FUNCTION_EXPRESSION(name, value) \
class name ## MoverFunctionExpression : public MoverFunctionExpression \
{ \
public: \
	double Execute(const MoverFunctionExpressionVariables& var) const override { return var.value; } \
}

SU_DEF_VARIABLE_MOVER_FUNCTION_EXPRESSION(Begin, Begin);
SU_DEF_VARIABLE_MOVER_FUNCTION_EXPRESSION(End, End);
SU_DEF_VARIABLE_MOVER_FUNCTION_EXPRESSION(Diff, Diff);
SU_DEF_VARIABLE_MOVER_FUNCTION_EXPRESSION(Current, Current);
SU_DEF_VARIABLE_MOVER_FUNCTION_EXPRESSION(Progress, Progress);

class LiteralMoverFunctionExpression : public MoverFunctionExpression
{
private:
    double value;
public:
    LiteralMoverFunctionExpression(double value) : value(value) {}
    double Execute(const MoverFunctionExpressionVariables& var) const override { return value; }
};

#define SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(name, value) \
class name ## MoverFunctionExpression : public MoverFunctionExpression \
{ \
public: \
	double Execute(const MoverFunctionExpressionVariables& var) const override { return value; } \
}

SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(E, M_E);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(LOG2E, M_LOG2E);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(LOG10E, M_LOG10E);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(LN2, M_LN2);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(LN10, M_LN10);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(PI, M_PI);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(PI_2, M_PI_2);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(PI_4, M_PI_4);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(INV_PI, M_1_PI);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(INV_PI_2, M_2_PI);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(INV_SQRTPI_2, M_2_SQRTPI);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(SQRT2, M_SQRT2);
SU_DEF_CONST_LITERAL_MOVER_FUNCTION_EXPRESSION(INV_SQRT2, M_SQRT1_2);

#define SU_DEF_SINGLE_OPERAND_MOVER_FUNCTION_EXPRESSION(name, op) \
class name ## MoverFunctionExpression : public MoverFunctionExpression \
{ \
private: \
	MoverFunctionExpressionSharedPtr pOp; \
public: \
	name ## MoverFunctionExpression(MoverFunctionExpressionSharedPtr &pOp) : pOp(pOp) {} \
	double Execute(const MoverFunctionExpressionVariables& var) const override { return op pOp->Execute(var); } \
}

SU_DEF_SINGLE_OPERAND_MOVER_FUNCTION_EXPRESSION(Positive, +);
SU_DEF_SINGLE_OPERAND_MOVER_FUNCTION_EXPRESSION(Negative, -);

#define SU_DEF_DOUBLE_OPERAND_MOVER_FUNCTION_EXPRESSION(name, op) \
class name ## MoverFunctionExpression : public MoverFunctionExpression \
{ \
private: \
	MoverFunctionExpressionSharedPtr pLop, pRop; \
public: \
	name ## MoverFunctionExpression(MoverFunctionExpressionSharedPtr &pLop, MoverFunctionExpressionSharedPtr &pRop) : pLop(pLop), pRop(pRop) {} \
	double Execute(const MoverFunctionExpressionVariables& var) const override { return pLop->Execute(var) op pRop->Execute(var); } \
}

SU_DEF_DOUBLE_OPERAND_MOVER_FUNCTION_EXPRESSION(Add, +);
SU_DEF_DOUBLE_OPERAND_MOVER_FUNCTION_EXPRESSION(Sub, -);
SU_DEF_DOUBLE_OPERAND_MOVER_FUNCTION_EXPRESSION(Mul, *);
SU_DEF_DOUBLE_OPERAND_MOVER_FUNCTION_EXPRESSION(Div, / );

#define SU_DEF_DOUBLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(name, func) \
class name ## MoverFunctionExpression : public MoverFunctionExpression \
{ \
private: \
	MoverFunctionExpressionSharedPtr pLop, pRop; \
public: \
	name ## MoverFunctionExpression(MoverFunctionExpressionSharedPtr &pLop, MoverFunctionExpressionSharedPtr &pRop) : pLop(pLop), pRop(pRop) {} \
	double Execute(const MoverFunctionExpressionVariables& var) const override { return func (pLop->Execute(var), pRop->Execute(var)); } \
}

SU_DEF_DOUBLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Mod, fmod);
SU_DEF_DOUBLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Pow, pow);
SU_DEF_DOUBLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Min, fmin);
SU_DEF_DOUBLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Max, fmax);
SU_DEF_DOUBLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Rand, rand);

#define SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(name, func) \
class name ## MoverFunctionExpression : public MoverFunctionExpression \
{ \
private: \
	MoverFunctionExpressionSharedPtr pArg; \
public: \
	name ## MoverFunctionExpression(MoverFunctionExpressionSharedPtr &pArg) : pArg(pArg) {} \
	double Execute(const MoverFunctionExpressionVariables& var) const override { return func (pArg->Execute(var)); } \
}

SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Abs, fabs);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Round, round);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Ceil, ceil);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Floor, floor);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Exp, exp);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Ln, log);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Log, log10);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Sin, sin);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Cos, cos);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Tan, tan);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Asin, asin);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Acos, acos);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Atan, atan);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Sinh, sinh);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Cosh, cosh);
SU_DEF_SINGLE_ARGUMENT_MOVER_FUNCTION_EXPRESSION(Tanh, tanh);

bool ParseMoverFunctionExpression(MoverFunctionExpressionSharedPtr &root, const std::string &);

MoverFunctionExpressionManager * MoverFunctionExpressionManager::inst;

bool MoverFunctionExpressionManager::Initialize()
{
    BOOST_ASSERT(!inst);
    if (!!inst) return false;

    inst = new MoverFunctionExpressionManager();
    BOOST_ASSERT(!!inst);

    return !!inst;
}

bool MoverFunctionExpressionManager::Finalize()
{
    BOOST_ASSERT(!!inst);
    if (!inst) return false;

    delete inst;

    return !inst;
}

bool MoverFunctionExpressionManager::Register(const std::string &key, const std::string &expression)
{
    BOOST_ASSERT(!!inst);
    if (!inst) return false;

    MoverFunctionExpressionSharedPtr pFunction;
    if (!ParseMoverFunctionExpression(pFunction, expression) || !pFunction) {
        spdlog::get("main")->error(u8"\"{0}\" 関数 (\"{1}\") の登録に失敗しました。", key, expression);
        return false;
    }

    return GetInstance().Register(key, pFunction);
}

bool MoverFunctionExpressionManager::Register(const std::string &key, MoverFunctionExpression *pFunction)
{
    const MoverFunctionExpressionSharedPtr ptr(pFunction);
    return Register(key, ptr);
}

bool MoverFunctionExpressionManager::Register(const std::string &key, const MoverFunctionExpressionSharedPtr &pFunction)
{
    if (list.find(key) != list.end()) {
        spdlog::get("main")->warn(u8"\"{0}\" 関数は既に登録されています。", key);
        return false;
    }

    list.insert(std::make_pair(key, pFunction));

    return true;
}

bool MoverFunctionExpressionManager::IsRegistered(const std::string &key)
{
    BOOST_ASSERT(!!inst);
    if (!inst) return false;

    MoverFunctionExpressionSharedPtr pFunction;
    const bool retVal = GetInstance().Find(key, pFunction);
    return retVal && pFunction;
}

bool MoverFunctionExpressionManager::Find(const std::string &key, MoverFunctionExpressionSharedPtr &pFunction) const
{
    const auto it = list.find(key);
    if (it == list.end()) return false;

    pFunction = it->second;
    return true;
}

#define BOOST_RESULT_OF_USE_DECLTYPE
#define BOOST_SPIRIT_USE_PHOENIX_V3

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>

template<typename T>
MoverFunctionExpressionSharedPtr MakeExp()
{
    return std::make_shared<T>();
}

template<typename T>
MoverFunctionExpressionSharedPtr MakeExp_double(double val)
{
    return std::make_shared<T>(val);
}

template<typename T>
MoverFunctionExpressionSharedPtr MakeExp_Ptr(MoverFunctionExpressionSharedPtr lop)
{
    return std::make_shared<T>(lop);
}

template<typename T>
MoverFunctionExpressionSharedPtr MakeExp_Ptr_Ptr(MoverFunctionExpressionSharedPtr lop, MoverFunctionExpressionSharedPtr rop)
{
    return std::make_shared<T>(lop, rop);
}

namespace parser_impl {
    using namespace boost::spirit;
    namespace phx = boost::phoenix;

    template<typename Iterator>
    struct mover_function_grammer
        : qi::grammar<Iterator, MoverFunctionExpressionSharedPtr(), ascii::space_type>
    {
        qi::rule<Iterator, MoverFunctionExpressionSharedPtr(), ascii::space_type> expr, term, fctr;

        mover_function_grammer() : mover_function_grammer::base_type(expr)
        {
            expr =           term[qi::_val = qi::_1]
                >> *(('+' >> term[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<AddMoverFunctionExpression>, qi::_val, qi::_1)])
                   | ('-' >> term[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<SubMoverFunctionExpression>, qi::_val, qi::_1)]));
            term =           fctr[qi::_val = qi::_1]
                >> *(('*' >> fctr[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<MulMoverFunctionExpression>, qi::_val, qi::_1)])
                   | ('/' >> fctr[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<DivMoverFunctionExpression>, qi::_val, qi::_1)]));
            fctr = qi::double_        [qi::_val = phx::bind(&MakeExp_double<LiteralMoverFunctionExpression>, qi::_1)]
                | (qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = qi::_1]
                | qi::lit("begin")       [qi::_val = phx::bind(&MakeExp<BeginMoverFunctionExpression>)]
                | qi::lit("end")         [qi::_val = phx::bind(&MakeExp<EndMoverFunctionExpression>)]
                | qi::lit("diff")        [qi::_val = phx::bind(&MakeExp<DiffMoverFunctionExpression>)]
                | qi::lit("current")     [qi::_val = phx::bind(&MakeExp<CurrentMoverFunctionExpression>)]
                | qi::lit("progress")    [qi::_val = phx::bind(&MakeExp<ProgressMoverFunctionExpression>)]
                | qi::lit('e')           [qi::_val = phx::bind(&MakeExp<EMoverFunctionExpression>)]
                | qi::lit("log2e")       [qi::_val = phx::bind(&MakeExp<LOG2EMoverFunctionExpression>)]
                | qi::lit("log10e")      [qi::_val = phx::bind(&MakeExp<LOG10EMoverFunctionExpression>)]
                | qi::lit("ln2")         [qi::_val = phx::bind(&MakeExp<LN2MoverFunctionExpression>)]
                | qi::lit("ln10")        [qi::_val = phx::bind(&MakeExp<LN10MoverFunctionExpression>)]
                | qi::lit("pi")          [qi::_val = phx::bind(&MakeExp<PIMoverFunctionExpression>)]
                | qi::lit("pi_2")        [qi::_val = phx::bind(&MakeExp<PI_2MoverFunctionExpression>)]
                | qi::lit("pi_4")        [qi::_val = phx::bind(&MakeExp<PI_4MoverFunctionExpression>)]
                | qi::lit("inv_pi")      [qi::_val = phx::bind(&MakeExp<INV_PIMoverFunctionExpression>)]
                | qi::lit("inv_pi_2")    [qi::_val = phx::bind(&MakeExp<INV_PI_2MoverFunctionExpression>)]
                | qi::lit("inv_sqrtpi_2")[qi::_val = phx::bind(&MakeExp<INV_SQRTPI_2MoverFunctionExpression>)]
                | qi::lit("sqrt2")       [qi::_val = phx::bind(&MakeExp<SQRT2MoverFunctionExpression>)]
                | qi::lit("inv_sqrt2")   [qi::_val = phx::bind(&MakeExp<INV_SQRT2MoverFunctionExpression>)]
                | (qi::lit("abs")   >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<AbsMoverFunctionExpression>,   qi::_1)]
                | (qi::lit("round") >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<RoundMoverFunctionExpression>, qi::_1)]
                | (qi::lit("ceil")  >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<CeilMoverFunctionExpression>,  qi::_1)]
                | (qi::lit("floor") >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<FloorMoverFunctionExpression>, qi::_1)]
                | (qi::lit("exp")   >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<ExpMoverFunctionExpression>,   qi::_1)]
                | (qi::lit("ln")    >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<LnMoverFunctionExpression>,    qi::_1)]
                | (qi::lit("log")   >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<LogMoverFunctionExpression>,   qi::_1)]
                | (qi::lit("sin")   >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<CosMoverFunctionExpression>,   qi::_1)]
                | (qi::lit("cos")   >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<SinMoverFunctionExpression>,   qi::_1)]
                | (qi::lit("tan")   >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<TanMoverFunctionExpression>,   qi::_1)]
                | (qi::lit("asin")  >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<AsinMoverFunctionExpression>,  qi::_1)]
                | (qi::lit("acos")  >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<AcosMoverFunctionExpression>,  qi::_1)]
                | (qi::lit("atan")  >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<AtanMoverFunctionExpression>,  qi::_1)]
                | (qi::lit("sinh")  >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<SinhMoverFunctionExpression>,  qi::_1)]
                | (qi::lit("cosh")  >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<CoshMoverFunctionExpression>,  qi::_1)]
                | (qi::lit("tanh")  >> qi::lit('(') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr<TanhMoverFunctionExpression>,  qi::_1)]
                | (qi::lit("add")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<AddMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("sub")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<SubMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("mul")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<MulMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("div")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<DivMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("mod")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<ModMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("pow")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<PowMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("min")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<MinMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("max")   >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<MaxMoverFunctionExpression>,  qi::_1, qi::_2)]
                | (qi::lit("rand")  >> qi::lit('(') >> expr >> qi::lit(',') >> expr >> qi::lit(')'))[qi::_val = phx::bind(&MakeExp_Ptr_Ptr<RandMoverFunctionExpression>, qi::_1, qi::_2)];
        }
    };

    mover_function_grammer<std::string::const_iterator> gMoverFunc;
}

bool ParseMoverFunctionExpression(MoverFunctionExpressionSharedPtr &root, const std::string &expression)
{
    return boost::spirit::qi::phrase_parse(expression.begin(), expression.end(), parser_impl::gMoverFunc, boost::spirit::ascii::space, root);
}


#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ros/ros.h>
#include <math.h>
#include <stdio.h>
#include <sensor_msgs/Imu.h>
#include "geometry_msgs/PoseStamped.h"
#include "mavros_msgs/Altitude.h"
#include "geometry_msgs/Vector3Stamped.h"

ros::Publisher acc_1;
ros::Publisher acc_2;
ros::Publisher veloc;

void imu_acc(const sensor_msgs::Imu::ConstPtr& msg)
{
	    q = Eigen::Quaternionf(imu.orientation.w, imu.orientation.x, imu.orientation.y, imu.orientation.z);
    R_mat= q.toRotationMatrix();

    ax=imu.linear_acceleration.x;
    ay=imu.linear_acceleration.y;
    az=imu.linear_acceleration.z;
    imuacc << ax,ay,az;
    acc= R_mat*imuacc ;
    imu_msg.pose.orientation.w = imu.orientation.w;
    imu_msg.pose.orientation.x = imu.orientation.x;
    imu_msg.pose.orientation.y = imu.orientation.y;
    imu_msg.pose.orientation.z = imu.orientation.z;
	return;
}

void vicon_vel(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
     if(count_vel==0)
     {
     	prev_x = msg->pose.position.x ;
     	prev_y = msg->pose.position.y ;
     	prev_z = msg->pose.position.z ;
     	count_vel++;
     	vel_last_time = ros::Time::now().toSec();
     }
     else
     {
     	dt_1 = msg->header.stamp.toSec() - vel_last_time;
     	vic_vel.header.stamp = ros::Time::now();
     	vic_vel.vector.x = (msg->pose.position.x - prev_x)/dt_1 ;
     	vic_vel.vector.y = (msg->pose.position.y - prev_y)/dt_1 ;
     	vic_vel.vector.z = (msg->pose.position.z - prev_z)/dt_1 ;

     	prev_x = msg->pose.position.x ;
     	prev_y = msg->pose.position.y ;
     	prev_z = msg->pose.position.z ;

     	veloc.publish(vic_vel);

     	vel_last_time = ros::Time::now().toSec();
     }

	return;
}

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

    ros::init(argc,argv,"check_acc");

    //Initializinging the parameters
    ros::NodeHandle nh;
    Initialize(nh);

    //Subscriber and Publisher for the data. remapped in the launch file to the topic required

    //ros::Duration(1).sleep();

    ros::Subscriber Imu = nh.subscribe("imu",10,imu_acc);
    ros::Subscriber vicon_1 = nh.subscribe("vicon_pos",10,vicon_vel);
    ros::Subscriber vicon_2 = nh.subscribe("vicon_vel",10,vicon_acc);

    acc_1 = nh.advertise<geometry_msgs::Vector3Stamped>("imu_acc", 10);
    acc_2 = nh.advertise<geometry_msgs::Vector3Stamped>("vicon_acc", 10);
    veloc = nh.advertise<geometry_msgs::Vector3Stamped>("vicon_velocity", 10);

    ros::Rate loop_rate(50);
    while(ros::ok()){

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <ecl/geometry/angle.hpp>
#include "plane_calibration/plane_to_depth_image.hpp"
#include "plane_calibration/plane_calibration.hpp"
#include "plane_calibration/calibration_parameters.hpp"
#include "plane_calibration/visualizer_interface.hpp"

using namespace plane_calibration;

TEST(PlaneCalibration, one_shot)
{
  int width = 640;
  int height = 480;
  double c_x = 321.3;
  double c_y = 212;
  double f_x = 570.3422;
  double f_y = 570.3422;
  CameraModel camera_model(c_x, c_y, f_x, f_y, width, height);

  double max_deviation = 0.1;
  double px = -0.628319;
  double py = 0.057;
  double pz = 0.0;
  Eigen::AngleAxisd start_rotation;
  start_rotation = Eigen::AngleAxisd(px, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(py, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(pz, Eigen::Vector3d::UnitZ());

  double px_offset = -0.023;
  double py_offset = 0.04;
  Eigen::AngleAxisd rotation_offset;
  rotation_offset = Eigen::AngleAxisd(px_offset, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(py_offset, Eigen::Vector3d::UnitY());

  Eigen::Vector3d ground_plane_offset(0.0, -0.16, 0.96);

  Eigen::Affine3d transform = Eigen::Translation3d(ground_plane_offset) * start_rotation * rotation_offset;
  Eigen::MatrixXf plane = PlaneToDepthImage::convert(transform, camera_model.getParameters());
  Eigen::MatrixXf noise = Eigen::MatrixXf::Random(plane.rows(), plane.cols());
  Eigen::MatrixXf random_plane_image = plane + 0.02 * noise;

  CalibrationParametersPtr parameters = std::make_shared<CalibrationParameters>();
  parameters->update(ground_plane_offset, max_deviation, start_rotation);

  VisualizerInterfacePtr dummy_visualizer;
  PlaneCalibrationPtr plane_calibration = std::make_shared<PlaneCalibration>(camera_model, parameters, dummy_visualizer);

  std::pair<double, double> one_shot_result = plane_calibration->calibrate(random_plane_image, 3);

  double estimated_px = one_shot_result.first;
  double estimated_py = one_shot_result.second;

  double epsilon = ecl::degrees_to_radians(0.5);
  EXPECT_NEAR(estimated_px, px_offset, epsilon);
  EXPECT_NEAR(estimated_py, py_offset, epsilon);
}


#include <armadillo>
#include <ForwardBackward.hpp>
#include <HSMM.hpp>
#include <iostream>
#include <json.hpp>
#include <memory>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace std;
using json = nlohmann::json;

void PrintBestWeCanAimFor(int nstates, int ndurations, int min_duration,
        field<ivec> hiddenStates, field<ivec> hiddenDurations) {
    int nseq = hiddenStates.n_elem;
    vec best_pi(nstates, fill::zeros);
    for(int s = 0; s < nseq; s++)
        best_pi(hiddenStates(s)(0))++;
    best_pi /= nseq;
    cout << "Best initial state pmf we can aim at:" << endl << best_pi << endl;

    cout << "Best transition matrix we can aim at:" << endl;
    mat prueba(nstates, nstates, fill::zeros);
    for(int s = 0; s < nseq; s++) {
        for(int i = 0; i < hiddenStates(s).n_elem - 1; i++)
            prueba(hiddenStates(s)(i), hiddenStates(s)(i + 1))++;
    }
    mat pruebasum = sum(prueba, 1);
    for(int i = 0; i < nstates; i++)
        prueba.row(i) /= pruebasum(i);
    cout << prueba << endl;

    cout << "Best duration matrix we can aim at:" << endl;
    mat emp_durations(nstates, ndurations, fill::zeros);
    for(int s = 0; s < nseq; s++) {
        for(int i = 0; i < hiddenStates(s).n_elem; i++)
            emp_durations(hiddenStates(s)(i), hiddenDurations(s)(i)
                    - min_duration)++;
    }
    mat emp_durations_sum = sum(emp_durations, 1);
    for(int i = 0; i < nstates; i++)
        emp_durations.row(i) /= emp_durations_sum(i);
    cout << emp_durations << endl;
}

void reset(HSMM& hsmm, vector<FullProMP> promps) {
    int nstates = hsmm.nstates_;
    int ndurations = hsmm.ndurations_;
    mat transition(hsmm.transition_);
    if (nstates == 1)
        transition.fill(1.0);  // Self-loops allowed in this case.
    else {
        transition.fill(1.0/(nstates-1));
        transition.diag().zeros();  // No self-loops.
    }
    hsmm.setTransition(transition);
    vec pi(hsmm.pi_);
    pi.fill(1.0/nstates);
    hsmm.setPi(pi);
    mat durations(hsmm.duration_);
    durations.fill(1.0/ndurations);
    hsmm.setDuration(durations);

    // Resetting emission.
    for(int i = 0; i < nstates; i++) {
        ProMP new_model = promps[i].get_model();
        vec new_mean = randn(size(new_model.get_mu_w()));
        mat new_Sigma_w(size(new_model.get_Sigma_w()), fill::eye);
        new_Sigma_w *= 10000;
        mat new_Sigma_y(size(new_model.get_Sigma_y()), fill::eye);
        new_Sigma_y *= 1;
        new_model.set_mu_w(new_mean);
        new_model.set_Sigma_w(new_Sigma_w);
        new_model.set_Sigma_y(new_Sigma_y);
        promps[i].set_model(new_model);
    }
    shared_ptr<AbstractEmission> ptr_emission(new ProMPsEmission(promps));
    hsmm.setEmission(ptr_emission);
}

int main() {
    int min_duration = 50;
    mat transition = {{0.0, 0.1, 0.4, 0.5},
                      {0.3, 0.0, 0.6, 0.1},
                      {0.2, 0.2, 0.0, 0.6},
                      {0.4, 0.4, 0.2, 0.0}};
    int nstates = transition.n_rows;
    vec pi = {0.1, 0.2, 0.3, 0.4};
    // mat durations(nstates, ndurations, fill::eye);
    mat durations =  {{0.0, 0.1, 0.4, 0.5},
                      {0.3, 0.0, 0.6, 0.1},
                      {0.2, 0.2, 0.0, 0.6},
                      {0.4, 0.4, 0.2, 0.0}};
    int ndurations = durations.n_cols;
    int n_basis_functions = 4;
    int njoints = 1;

    // Setting a third order polynomial basis function for the ProMP
    int polynomial_order = n_basis_functions - 1;
    shared_ptr<ScalarBasisFun> kernel{ new ScalarPolyBasis(polynomial_order)};

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w(n_basis_functions * njoints);
        mu_w.fill(i * 10);
        mat Sigma_w = (i + 1) * eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.0001*eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(kernel, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission.
    shared_ptr<AbstractEmission> ptr_emission(new ProMPsEmission(promps));

    HSMM promp_hsmm(ptr_emission, transition, pi, durations, min_duration);

    int nseq = 1;
    int nsegments = 50;
    field<ivec> hidden_states, hidden_durations;
    field<field<mat>> multiple_toy_obs = promp_hsmm.sampleMultipleSequences(
            nseq, nsegments, hidden_states, hidden_durations);
    cout << "Generated states and durations for the first sequence" << endl;
    cout << join_horiz(hidden_states(0), hidden_durations(0)) << endl;

    PrintBestWeCanAimFor(nstates, ndurations, min_duration, hidden_states,
            hidden_durations);

    cout << "Original emission parameters" << endl;
    json params_test = promp_hsmm.emission_->to_stream();
    cout << params_test.dump(4) << endl;

    reset(promp_hsmm, promps);

    cout << "Emission parameters after reset" << endl;
    params_test = promp_hsmm.emission_->to_stream();
    cout << params_test.dump(4) << endl;

    // Providing some sparse labels.
    set<int> observed_indexes = {};  //{5,6,7,8,25,26,32,33,39,41,42,43,47};
    field<Labels> mlabels(nseq);
    for(int s = 0; s < nseq; s++) {
        int idx = 0;
        Labels observed_segments;
        for(int i = 0; i < nsegments; i++) {
            int hs = hidden_states(0)(i);
            int dur = hidden_durations(0)(i);
            idx += dur;
            if (observed_indexes.find(i) != observed_indexes.end()) {
                cout << "label hs: " << hs << endl;
                observed_segments.setLabel(idx - 1, dur, hs);
            }
        }
        mlabels(s) = observed_segments;
    }

    // Learning the model from data.
    promp_hsmm.fit(multiple_toy_obs, mlabels, 100, 1e-10);

    cout << "Model parameters after training" << endl;
    json params = promp_hsmm.to_stream();
    cout << params.dump(4) << endl;

    // Running the Viterbi algorithm for the first sequence.
    const field<mat>& toy_obs = multiple_toy_obs(0);
    imat psi_duration(nstates, toy_obs.n_elem, fill::zeros);
    imat psi_state(nstates, toy_obs.n_elem, fill::zeros);
    mat delta(nstates, toy_obs.n_elem, fill::zeros);
    cube log_pdf = promp_hsmm.computeEmissionsLogLikelihood(toy_obs);
    Viterbi(transition, pi, durations, log_pdf, delta, psi_duration, psi_state,
            min_duration, toy_obs.n_elem);
    cout << "Delta last column" << endl;
    cout << delta.col(toy_obs.n_elem - 1) << endl;
    ivec viterbiStates, viterbiDurations;
    viterbiPath(psi_duration, psi_state, delta, viterbiStates,
            viterbiDurations);

    cout << "Viterbi states and durations" << endl;
    cout << join_horiz(viterbiStates, viterbiDurations) << endl;
    int dur_diff = 0;
    int states_diff = 0;
    for(int i = 0; i < viterbiDurations.n_elem; i++) {
        dur_diff += (viterbiDurations[i] != hidden_durations(0)(i));
        states_diff += (viterbiStates[i] != hidden_states(0)(i));
    }
    cout << "The number of mismatches in duration is " << dur_diff << endl;
    cout << "The number of mismatches in hidden states is " << states_diff <<
            endl;
    return 0;
}


#include <iostream>
#include <string>
#include <fstream>
#include <chrono>
#include <random>

#include <MLearn/Core>
#include <MLearn/NeuralNets/layers/fc_layer.h>
#include <MLearn/NeuralNets/neural_nets.h>
#include <MLearn/Optimization/StochasticGradientDescent.h>
#include <MLearn/Optimization/AdaGrad.h>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <CImg.h>

using namespace std;
using namespace MLearn;
using namespace nn;
using namespace Optimization;
namespace fs = boost::filesystem;
using namespace cimg_library;
typedef double float_type;

// import MNIST - for INTEL processor (or other little-endian processors)
int importMNIST( const fs::path& images_path, const fs::path& labels_path, MLMatrix< float_type >& images, MLMatrix< float_type >& output ){

	ifstream imageFile;
	ifstream labelFile;

	// read images file
	imageFile.open( images_path.string(), ios::binary );
	labelFile.open( labels_path.string(), ios::binary );
	// check if open succeded
	if (imageFile.fail() || labelFile.fail()){
		throw "Error opening the file!";
	}

	// useful variables
	unsigned char byte;

	// read out magic numbers
	uint32_t magic_number = 0;
	uint32_t magic_number_label = 0;
	imageFile.read((char*)&byte,sizeof(byte));
	magic_number |= ( static_cast<uint32_t>(byte) << 24 );
	imageFile.read((char*)&byte,sizeof(byte));
	magic_number |= ( static_cast<uint32_t>(byte) << 16 );
	imageFile.read((char*)&byte,sizeof(byte));
	magic_number |= ( static_cast<uint32_t>(byte) << 8 );
	imageFile.read((char*)&byte,sizeof(byte));
	magic_number |= ( static_cast<uint32_t>(byte) );
	labelFile.read((char*)&byte,sizeof(byte));
	magic_number_label |= ( static_cast<uint32_t>(byte) << 24 );
	labelFile.read((char*)&byte,sizeof(byte));
	magic_number_label |= ( static_cast<uint32_t>(byte) << 16 );
	labelFile.read((char*)&byte,sizeof(byte));
	magic_number_label |= ( static_cast<uint32_t>(byte) << 8 );
	labelFile.read((char*)&byte,sizeof(byte));
	magic_number_label |= ( static_cast<uint32_t>(byte) );

	cout << "Sanity check: Magic number = " << magic_number << std::endl;
	cout << "Sanity check: Magic number label = " << magic_number_label << std::endl;
	if ( (magic_number != 2051) || (magic_number_label != 2049) ){
		throw "Error: magic number not correct!";
	}

	// get number of images
	uint32_t N_images = 0;
	uint32_t N_labels = 0;
	imageFile.read((char*)&byte,sizeof(byte));
	N_images |= ( static_cast<uint32_t>(byte) << 24 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_images |= ( static_cast<uint32_t>(byte) << 16 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_images |= ( static_cast<uint32_t>(byte) << 8 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_images |= ( static_cast<uint32_t>(byte) );
	cout << "Number of images detected = " << N_images << endl;
	labelFile.read((char*)&byte,sizeof(byte));
	N_labels |= ( static_cast<uint32_t>(byte) << 24 );
	labelFile.read((char*)&byte,sizeof(byte));
	N_labels |= ( static_cast<uint32_t>(byte) << 16 );
	labelFile.read((char*)&byte,sizeof(byte));
	N_labels |= ( static_cast<uint32_t>(byte) << 8 );
	labelFile.read((char*)&byte,sizeof(byte));
	N_labels |= ( static_cast<uint32_t>(byte) );
	cout << "Number of labels detected = " << N_labels << endl;

	if (N_images != N_labels){
		throw "Error: different number of training examples detected!";
	}

	// get number of rows and cols
	uint32_t N_rows = 0;
	imageFile.read((char*)&byte,sizeof(byte));
	N_rows |= ( static_cast<uint32_t>(byte) << 24 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_rows |= ( static_cast<uint32_t>(byte) << 16 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_rows |= ( static_cast<uint32_t>(byte) << 8 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_rows |= ( static_cast<uint32_t>(byte) );
	uint32_t N_cols = 0;
	imageFile.read((char*)&byte,sizeof(byte));
	N_cols |= ( static_cast<uint32_t>(byte) << 24 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_cols |= ( static_cast<uint32_t>(byte) << 16 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_cols |= ( static_cast<uint32_t>(byte) << 8 );
	imageFile.read((char*)&byte,sizeof(byte));
	N_cols |= ( static_cast<uint32_t>(byte) );

	cout << "Image dimensions: "<<N_rows<<"x"<<N_cols<<endl;

	images.resize(N_rows*N_cols,N_images);
	output = MLMatrix<float_type>::Constant(10,N_images,0);

	// use openCV matrix
/*	Mat image = Mat::zeros(N_rows,N_cols,CV_8UC1);
	Mat_<double> image_to_eigen_gray;
	MLMatrix<double> eigen_image(N_rows,N_cols);
	Eigen::Map< MLVector<double> > view(eigen_image.data(), N_rows*N_cols);*/

	for (uint32_t i = 0; i < N_images; ++i){

		for (uint32_t r = 0; r < N_rows; ++r){

			for (uint32_t c = 0; c < N_cols; ++c){

				imageFile.read((char*)&byte,sizeof(byte));
				//image.at<uchar>(r,c) = byte;
				images(c*N_rows + r, i) = float_type(byte);
				if (imageFile.fail()){
					throw "Error reading the file!";
				}

			}

		}

		// read label
		labelFile.read((char*)&byte,sizeof(byte));
		if (labelFile.fail()){
			throw "Error reading the file!";
		}
		output.col(i)[unsigned(byte)] = 1;

		// transform the image in the range 0 - 1
		//normalize(image,image_to_eigen_gray,0,1,NORM_MINMAX,CV_64FC1);
		//cv2eigen(image_to_eigen_gray,eigen_image);
		//images.col(i) = view;
	}
	images.array() /= 255.0;

	imageFile.close();
	labelFile.close();

	return N_rows;

}

int main(int argc, char* argv[]){
	std::srand((unsigned int) time(0));

	namespace po = boost::program_options;
	typedef MLMatrix<float_type> Matrix;
	typedef MLVector<float_type> Vector;
	constexpr ActivationType type = ActivationType::TANH;
	constexpr LossType loss_t = LossType::SOFTMAX_CROSS_ENTROPY;
	typedef FCLayer<float_type, type> layer_t;

	// Create command line options
	po::options_description
	desc("This is a demo showing a simple FC neural net trained on MNIST.");

	desc.add_options()
		("help", "Show the help")
		("data_folder", po::value<string>(), "Folder where to find the uncompressed data.")
		("visualize,v", po::bool_switch()->default_value(false), "Visualize test samples with classification.");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << "\n";
		return 1;
	}
	string data_folder;
	if (vm.count("data_folder")){
		data_folder = vm["data_folder"].as<string>();
	}else{
		throw "Invalid data folder";
		return 1;
	}
	fs::path data_folder_path(data_folder);
	bool visualize = vm["visualize"].as<bool>();

	Matrix train_images;
	Matrix test_images;
	Matrix train_labels;
	Matrix test_labels;
	Matrix labels;
	Vector temp;

	int image_rows = importMNIST(data_folder_path/fs::path("train-images.idx3-ubyte"),
								 data_folder_path/fs::path("train-labels.idx1-ubyte"),
								 train_images, train_labels);
	int image_cols = train_images.rows()/image_rows;
	importMNIST(data_folder_path/fs::path("t10k-images.idx3-ubyte"), data_folder_path/fs::path("t10k-labels.idx1-ubyte"),
				test_images, test_labels);

	Vector real_labels = Vector::Zero(test_labels.cols());
	Vector hat_labels = Vector::Zero(test_labels.cols());

	for (int idx = 0; idx < test_labels.cols(); ++idx){
		temp = test_labels.col(idx);
		temp.maxCoeff(&real_labels[idx]);
	}

	auto network = make_network<float_type, loss_t>(layer_t(100), layer_t(50), layer_t(train_labels.rows()));
	network.set_input_dim(train_images.rows());
	Vector weights = Vector::Random(network.get_n_parameters())*2.5;
	network.set_weights(weights.data(), true);

	LineSearch< LineSearchStrategy::FIXED,float_type,uint > line_search(0.3);
	Optimization::AdaGrad<LineSearchStrategy::FIXED,float_type,uint,0> minimizer;
	minimizer.setMaxIter(10);
	minimizer.setMaxEpoch(1);
	minimizer.setSizeBatch(500);
	minimizer.setNSamples(train_images.cols());
	minimizer.setLineSearchMethod(line_search);
	minimizer.setSeed(std::chrono::system_clock::now().time_since_epoch().count());
	std::cout << "Initial loss: " << network.evaluate(test_images, test_labels) << std::endl;
	int n_epochs = 0;

	random_device rand_dev;
	mt19937 generator(rand_dev());
	uniform_int_distribution<int> dist(0, test_images.cols());
	char title[50];
	CImgDisplay main_disp(800, 800,"Test samples", 3, false, true);
	while (true){
		++n_epochs;
		network.fit(train_images, train_labels, minimizer);
		labels = network.forward_pass(test_images);
		float_type accuracy = 0;
		for (int idx = 0; idx < test_labels.cols(); ++idx){
			temp = labels.col(idx);
			temp.maxCoeff(&hat_labels[idx]);
			accuracy += float_type( int(hat_labels[idx]) == int(real_labels[idx]) );
		}
		float_type loss = network.evaluate(test_images, test_labels);
		accuracy /= float_type(test_labels.cols());
		float_type error_rate = 1.0 - accuracy;
		std::cout << "Loss: " << loss;
		std::cout << " Error rate: " << error_rate << std::endl;
		if (visualize && (n_epochs % 10 == 0)){
				int n_images_to_show_per_dim = 10; // This shows n*n images
				int frame_width = 2; // surround images with a frame

			    // Create empty yellow image
			    CImg<float_type> image(n_images_to_show_per_dim*(image_cols + 2*frame_width),
			    					   n_images_to_show_per_dim*(image_rows + 2*frame_width), 1, 3);
			    image = 0.0;
			    // fill the image
			    float red[]  = { 1.0,0,0 };
			    float green[]  = { 0.0,1.0,0 };
			    for (int i_grid = 0; i_grid < n_images_to_show_per_dim; ++i_grid){
			    	for (int j_grid = 0; j_grid < n_images_to_show_per_dim; ++j_grid){
			    		int sample_idx = dist(generator);

			    		// Draw sample
			    		int top_left_col = j_grid*(2*frame_width + image_cols) + 2;
			    		int top_left_row = i_grid*(2*frame_width + image_rows) + 2;
			    		for (int i = 0; i < image_rows; ++i){
			    			for (int j = 0; j < image_cols; ++j){
			    				int pixel_row = top_left_row + i;
			    				int pixel_col = top_left_col + j;

			    				image(pixel_row, pixel_col, 0, 0) = test_images(i*image_rows + j, sample_idx);
			    				image(pixel_row, pixel_col, 0, 1) = test_images(i*image_rows + j, sample_idx);
			    				image(pixel_row, pixel_col, 0, 2) = test_images(i*image_rows + j, sample_idx);

			    			}
			    		}
	    				if (hat_labels[sample_idx] == real_labels[sample_idx]){
	    					image.draw_text(top_left_row, top_left_col, std::to_string(int(hat_labels[sample_idx])).c_str(), green);
	    				}else{
	    					image.draw_text(top_left_row, top_left_col, std::to_string(int(hat_labels[sample_idx])).c_str(), red);
	    				}
			    	}
			    }

			    image.resize(800, 800);
			    main_disp.display(image);
			    sprintf(title, "Test samples - Loss: %.3f - Error rate: %.3f", loss, error_rate);
			    main_disp.set_title(title);
			    main_disp.show();
		}
	}

	return 0;
}


/***********************************************************************
Copyright 2018 Gregory Bryant

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
***********************************************************************/

#include "neconsolecommand.h"
#include "neconsolewindow.h"
#include "nemainwindow.h"
#include <boost/multiprecision/gmp.hpp>

using boost::multiprecision::abs;
using boost::multiprecision::mpz_int;

class ToggleBoolean : public NEConsoleCommand
{
public:

    QString label;
    QString aliasLabel;
    bool    *value;

    ToggleBoolean(NEConsoleWindow *consoleWindow,bool *boolVal, QString label, QString aliasLabel):NEConsoleCommand(consoleWindow)
    {
        name = "toggle";
        alias = "t";
        description = "toggle/t "+label+"/"+aliasLabel;
        this->label = label;
        this->value = boolVal;
        this->aliasLabel = aliasLabel;
    }
    void command(void *data);
};

void ToggleBoolean::command(void *data)
{
    QString *txt = (QString*)data;
    if(txt==label||txt==aliasLabel)
    {
        (*value) = !(*value);
        consoleWindow->appendText(label+": "+QString::number(*value)+"\n");
        consoleWindow->mainWindow->view.render();
        consoleWindow->mainWindow->view.update();
        consoleWindow->lineEdit.setText("");
    }
}

class GoX : public NEConsoleCommand
{
public:
    GoX(NEConsoleWindow *consoleWindow):NEConsoleCommand(consoleWindow)
    {
        name = "x";
        description = "x n go to x coordinate n";
    }
    void command(void *data);
};

void GoX::command(void *data)
{
    QString *txt = (QString*)data;

    mpz_int xPos;
    int err = mpz_set_str(xPos.backend().data(),txt->toStdString().c_str(),10);
    if(err){consoleWindow->lineEdit.setText("failure");return;}

    consoleWindow->mainWindow->view.setXPos(xPos);

    consoleWindow->mainWindow->view.render();
    consoleWindow->mainWindow->view.update();
    consoleWindow->lineEdit.setText("");
}

class GoY : public NEConsoleCommand
{
public:
    GoY(NEConsoleWindow *consoleWindow):NEConsoleCommand(consoleWindow)
    {
        name = "y";
        description = "y n go to y coordinate n";
    }
    void command(void *data);
};

void GoY::command(void *data)
{
    QString *txt = (QString*)data;

    mpz_int yPos;
    int err = mpz_set_str(yPos.backend().data(),txt->toStdString().c_str(),10);
    if(err){consoleWindow->lineEdit.setText("failure");return;}

    consoleWindow->mainWindow->view.setYPos(yPos);

    consoleWindow->mainWindow->view.render();
    consoleWindow->mainWindow->view.update();
    consoleWindow->lineEdit.setText("");
}

class GoScale : public NEConsoleCommand
{
public:
    GoScale(NEConsoleWindow *consoleWindow):NEConsoleCommand(consoleWindow)
    {
        name = "zoom";
        alias = "z";
        description = "zoom n set scale to n";
    }
    void command(void *data);
};

void GoScale::command(void *data)
{
    QString *txt = (QString*)data;

    int scale = txt->toInt();
    if(scale<=0){scale=2;}

    consoleWindow->mainWindow->view.setScale(scale);

    consoleWindow->mainWindow->view.render();
    consoleWindow->mainWindow->view.update();
    consoleWindow->lineEdit.setText("");
}

class PrintInfo : public NEConsoleCommand
{
public:
    PrintInfo(NEConsoleWindow *consoleWindow):NEConsoleCommand(consoleWindow)
    {
        name = "p";
        description = "print various pieces of debug information";
    }
    void command(void *data);
};

void PrintInfo::command(void *data)
{
    mpz_int left;
    mpz_int right;
    mpz_int top;
    mpz_int bottom;
    std::stringstream xStream;
    std::stringstream yStream;
    consoleWindow->mainWindow->view.getScreenBounds(&top,&bottom,&left,&right);
    consoleWindow->appendText("\n");
    QString outStr = QString("scale: ");
    outStr += QN(consoleWindow->mainWindow->view.scale);
    outStr += "\n";
    outStr += QString("resolution: ");
    outStr += QN(consoleWindow->mainWindow->view.resolution);
    outStr+="\n";
    xStream.str("");
    yStream.str("");
    xStream<<top;
    yStream<<bottom;
    outStr+="top: "+QString::fromStdString(xStream.str());
    outStr+=" bottom: "+QString::fromStdString(yStream.str());
    outStr+="\n";
    xStream.str("");
    yStream.str("");
    xStream<<left;
    yStream<<right;
    outStr+="left: "+QString::fromStdString(xStream.str());
    outStr+=" right: "+QString::fromStdString(yStream.str());
    outStr+="\n";
    outStr+="xFactorialOffset: "+QN(consoleWindow->mainWindow->view.xFactorialOffset);
    outStr+="\n";
    outStr+="yFactorialOffset: "+QN(consoleWindow->mainWindow->view.yFactorialOffset);
    outStr+="\n";
    xStream.str("");
    yStream.str("");
    xStream<<consoleWindow->mainWindow->view.xOffset;
    yStream<<consoleWindow->mainWindow->view.yOffset;
    outStr+="xOffset:"+QString::fromStdString(xStream.str());
    outStr+="\n";
    outStr+="yOffset:"+QString::fromStdString(yStream.str());
    outStr+="\n";
    xStream.str("");
    yStream.str("");
    xStream<<consoleWindow->mainWindow->view.xScrollAmt;
    yStream<<consoleWindow->mainWindow->view.yScrollAmt;
    outStr+="xScrollAmt:"+QString::fromStdString(xStream.str());
    outStr+="\n";
    outStr+="yScrollAmt:"+QString::fromStdString(yStream.str());
    outStr+="\n";
    consoleWindow->appendText(outStr);
    consoleWindow->appendText("center point: ");
    consoleWindow->mainWindow->view.printScreenPoint(consoleWindow->mainWindow->view.width()/2,consoleWindow->mainWindow->view.height()/2);
    consoleWindow->appendText("\n");
    consoleWindow->lineEdit.setText("");
}

class QuitApp : public NEConsoleCommand
{
public:
    QuitApp(NEConsoleWindow *consoleWindow):NEConsoleCommand(consoleWindow)
    {
        name = "quit";
        alias = "q";
        description = "quit application";
    }
    void command(void *data);
};

void QuitApp::command(void *data)
{
    qApp->exit();
    consoleWindow->lineEdit.setText("");
}

class AppKeys : public NEConsoleCommand
{
public:
    AppKeys(NEConsoleWindow *consoleWindow):NEConsoleCommand(consoleWindow)
    {
        name = "keys";
        description = "prints a list of keystrokes";
    }
    void command(void *data);
};

void AppKeys::command(void *data)
{
    consoleWindow->appendText("\n");
    consoleWindow->appendText("control + \t--- zoom in\n");
    consoleWindow->appendText("control - \t--- zoom out\n");
    //consoleWindow->appendText("alt + \t--- increase resolution\n");
    //consoleWindow->appendText("alt - \t--- decrease resolution\n");
    consoleWindow->appendText("arrow keys --- scroll around by 10 pixels\n");
    consoleWindow->appendText("arrow keys with shift  --- scroll around by 1 pixel\n");
    consoleWindow->appendText("arrow keys with control --- scroll around by 100 pixels\n");
    consoleWindow->appendText("arrow keys with control and shift --- scroll around by 500 pixels\n");
    consoleWindow->appendText("arrow keys with alt --- scroll by factorials\n");
    consoleWindow->appendText("space bar \t--- click point under crosshair\n");
    consoleWindow->lineEdit.setText("");
}

/*
class DumpData : public ConsoleCommand
{
public:
    DumpData(ConsoleWindow *consoleWindow):ConsoleCommand(consoleWindow)
    {
        name = "dump";
        description = "dump data points";
    }
    void command(void *data);
};

void DumpData::command(void *data)
{
    for(int i=0;i<cw->displayWidget->displayObjects.count();i++)
    {
        if(cw->displayWidget->displayObjects[i]->name=="PointData")
        {
            for(int j=0;j<((PointData*)cw->displayWidget->displayObjects[i])->points.count();j++)
            {
                float a = ((PointData*)cw->displayWidget->displayObjects[i])->points[j].first;
                float b = ((PointData*)cw->displayWidget->displayObjects[i])->points[j].second;
                cw->appendText(QN(a)+","+QN(b)+"\n");
            }
        }
    }
    cw->ui->lineEdit->setText("");
}
*/

class TestCommand : public NEConsoleCommand
{
public:
    TestCommand(NEConsoleWindow *consoleWindow):NEConsoleCommand(consoleWindow)
    {
        name = "test";
        alias = "t";
        description = "test command";
    }
    void command(void *data);
};

void TestCommand::command(void *data)
{

}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__MPC__CLTV_OCP_HPP_
#define CBR_CONTROL__MPC__CLTV_OCP_HPP_

#include <Eigen/Dense>

#include <cbr_utils/utils.hpp>

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>

#include <utility>

#include "ocp_common.hpp"

namespace cbr
{

/* ---------------------------------------------------------------------------------------------- */
/*                          Nonlinear Optimal Control Problem Linearizer                          */
/* ---------------------------------------------------------------------------------------------- */

struct CltvOcpParams
{
};

/**
 * @brief Linearize a nonlinear control problem to a continuous linear time-varying problem
 * @tparam nl_pb_t nonlinear problem satisfying interface conditions
 */
template<typename nl_pb_t>
class CltvOcp
{
public:
  // Must be defined in nl_pb_t problem
  constexpr static std::size_t nx = nl_pb_t::nx;
  constexpr static std::size_t nu = nl_pb_t::nu;

  // Create some useful aliases
  using state_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // Get return type of problem functions
  using Tr_t = std::result_of_t<decltype(&nl_pb_t::get_T)(nl_pb_t)>;
  using x0r_t = std::result_of_t<decltype(&nl_pb_t::get_x0)(nl_pb_t)>;
  using xlr_t = std::result_of_t<decltype(&nl_pb_t::get_xl)(nl_pb_t, double)>;
  using ulr_t = std::result_of_t<decltype(&nl_pb_t::get_ul)(nl_pb_t, double)>;
  using xdr_t = std::result_of_t<decltype(&nl_pb_t::get_xd)(nl_pb_t, double)>;
  using udr_t = std::result_of_t<decltype(&nl_pb_t::get_ud)(nl_pb_t, double)>;
  using Qr_t = std::result_of_t<decltype(&nl_pb_t::get_Q)(nl_pb_t, double)>;
  using QTr_t = std::result_of_t<decltype(&nl_pb_t::get_QT)(nl_pb_t)>;
  using Rr_t = std::result_of_t<decltype(&nl_pb_t::get_R)(nl_pb_t, double)>;

  // Check return type of problem functions
  static_assert(
    std::is_same_v<std::decay_t<Tr_t>, double>,
    "The get_T method of the problem must return a double (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<x0r_t>, state_t>,
    "The get_x0 method of the problem must return an nx*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<xlr_t>, state_t>,
    "The get_xl method of the problem must return an nx*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<ulr_t>, input_t>,
    "The get_ul method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<xdr_t>, state_t>,
    "The get_xd method of the problem must return an nx*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<udr_t>, input_t>,
    "The get_ud method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Qr_t>, Q_t>,
    "The get_Q method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Rr_t>, R_t>,
    "The get_R method of the problem must return an nu*nu Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<QTr_t>, Q_t>,
    "The get_QT method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");

  // Check problem dimensions
  static_assert(nx > 0, "Number of states must be > 0.");
  static_assert(nu > 0, "Number of inputs must be > 0.");

public:
  CltvOcp() = delete;
  CltvOcp(const CltvOcp &) = default;
  CltvOcp(CltvOcp &&) = default;
  CltvOcp & operator=(const CltvOcp &) = default;
  CltvOcp & operator=(CltvOcp &&) = default;

  explicit CltvOcp(const nl_pb_t & pb)
  : nl_pb_(pb)
  {}

  explicit CltvOcp(nl_pb_t && pb)
  : nl_pb_(std::move(pb))
  {}

  template<typename T1>
  CltvOcp(T1 && pb)
  : nl_pb_(std::forward<T1>(pb))
  {}

  void get_x0(Eigen::Ref<state_t> x0) const
  {
    x0 = nl_pb_.get_x0();
    x0 -= nl_pb_.get_xl(0.);
  }

  void get_T(double & t) const
  {
    t = nl_pb_.get_T();
  }

  void get_state_lb(double t, Eigen::Ref<state_t> state_lb) const
  {
    nl_pb_.get_state_lb(t, state_lb);
    state_lb -= nl_pb_.get_xl(t);
  }

  void get_state_ub(double t, Eigen::Ref<state_t> state_ub) const
  {
    nl_pb_.get_state_ub(t, state_ub);
    state_ub -= nl_pb_.get_xl(t);
  }

  void get_input_lb(double t, Eigen::Ref<input_t> input_lb) const
  {
    nl_pb_.get_input_lb(t, input_lb);
    input_lb -= nl_pb_.get_ul(t);
  }

  void get_input_ub(double t, Eigen::Ref<input_t> input_ub) const
  {
    nl_pb_.get_input_ub(t, input_ub);
    input_ub -= nl_pb_.get_ul(t);
  }

  A_t get_A(double t) const
  {
    using X_t = Eigen::Matrix<autodiff::dual, nx, 1>;
    X_t xlin = nl_pb_.get_xl(t);
    const ulr_t u = nl_pb_.get_ul(t);

    auto fx = [&](const X_t & x) -> X_t {
        return nl_pb_.get_f(x, u);
      };

    return autodiff::forward::jacobian(fx, autodiff::wrt(xlin), autodiff::forward::at(xlin));
  }

  B_t get_B(double t) const
  {
    using X_t = Eigen::Matrix<autodiff::dual, nx, 1>;
    using U_t = Eigen::Matrix<autodiff::dual, nu, 1>;
    U_t ulin = nl_pb_.get_ul(t);
    const xlr_t x = nl_pb_.get_xl(t);

    auto fu = [&](const U_t & u) -> X_t {
        return nl_pb_.get_f(x, u);
      };

    return autodiff::forward::jacobian(fu, autodiff::wrt(ulin), autodiff::forward::at(ulin));
  }

  state_t get_E(double t) const
  {
    using xldotr_t = std::result_of_t<decltype(&nl_pb_t::get_xldot)(nl_pb_t, double)>;

    const xlr_t xl = nl_pb_.get_xl(t);
    const xldotr_t xlDot = nl_pb_.get_xldot(t);
    const ulr_t ul = nl_pb_.get_ul(t);
    return nl_pb_.get_f(xl, ul) - xlDot;
  }

  Qr_t get_Q(double t) const
  {
    return nl_pb_.get_Q(t);
  }

  QTr_t get_QT() const
  {
    return nl_pb_.get_QT();
  }

  Rr_t get_R(double t) const
  {
    return nl_pb_.get_R(t);
  }

  state_t get_q(double t) const
  {
    const xlr_t xl = nl_pb_.get_xl(t);
    const xdr_t xd = nl_pb_.get_xd(t);
    const Qr_t Q = nl_pb_.get_Q(t);
    return (xl - xd).transpose() * Q;
  }

  state_t get_qT() const
  {
    double T = nl_pb_.get_T();
    const xlr_t xl = nl_pb_.get_xl(T);
    const xdr_t xd = nl_pb_.get_xd(T);
    const QTr_t QT = nl_pb_.get_QT();
    return (xl - xd).transpose() * QT;
  }

  input_t get_r(double t) const
  {
    const ulr_t ul = nl_pb_.get_ul(t);
    const udr_t ud = nl_pb_.get_ud(t);
    const Rr_t R = nl_pb_.get_R(t);
    return (ul - ud).transpose() * R;
  }

  nl_pb_t & problem()
  {
    return nl_pb_;
  }

protected:
  nl_pb_t nl_pb_{};
};

// Class template argument deduction guides
template<typename T>
CltvOcp(T)->CltvOcp<T>;

template<typename T1, typename T2>
CltvOcp(T1, T2)->CltvOcp<T1>;

}  // namespace cbr

#endif  // CBR_CONTROL__MPC__CLTV_OCP_HPP_


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#define BOOST_ERROR_CODE_HEADER_ONLY
#define BOOST_CHRONO_HEADER_ONLY
#define BOOST_CHRONO_EXTENSIONS
#include <boost/chrono.hpp>
using namespace boost;
using namespace boost::chrono;

//////////////////////////////////////////
typedef duration<long,   ratio<30>> half_min;
typedef duration<int,    ratio<60*15>> quater;
typedef duration<double, ratio<3600*24>> day;

//typedef duration<int,60*60> my_hour;
//typedef duration<int,ratio<-10, 1000>> my_ms;
void case1()
{
    seconds s(10);
    minutes m(5);
    hours   h(1);
    milliseconds ms(100);

    assert(s.count() == 10);
    assert(ms.count() == 100);

    s *= 3;
    s += seconds(30);
    s = s - seconds(20);
    assert(s < seconds(50));
    cout << s << endl;

}

//////////////////////////////////////////
void case2()
{
    seconds s(10);
    minutes m(5);

    s += m;
    cout << s << endl;

    //m+= s;

    {
        seconds s(10);
        typedef duration<double, ratio<60>> my_min;
        my_min m(5);
        m += s;
        cout << m << endl;
    }

    {
        seconds s(40);
        auto m = duration_cast<minutes>(s);
        cout << m << endl;

        seconds s2(301);
        cout << duration_cast<minutes>(s2) << endl;
    }

    {
        seconds s(3600 + 50);
        cout << floor<minutes>(s) << endl;
        cout << ceil<minutes>(s) << endl;
        cout << round<minutes>(s) << endl;
        cout << round<hours>(s) << endl;
    }
}

//////////////////////////////////////////
template<typename T>
using clock_desc = clock_string<T, char>;

void case3()
{
    cout << clock_desc<system_clock>::name() << endl;
    cout << clock_desc<system_clock>::since() << endl;

    cout << clock_desc<steady_clock>::name() << endl;
    cout << clock_desc<steady_clock>::since() << endl;

    cout << clock_desc<process_real_cpu_clock>::name() << endl;
    cout << clock_desc<process_real_cpu_clock>::since() << endl;
}

//////////////////////////////////////////
void case4()
{
    auto tp1 = system_clock::now();
    cout << tp1 << endl;

    auto d = tp1.time_since_epoch();
    cout << duration_cast<hours>(d) << endl;
    cout << duration_cast<day>(d) << endl;

    auto tp2 = tp1 +minutes(1);
    cout << tp2 << endl;

    {
        auto tp = steady_clock::now();
        cout << tp << endl;

        auto d = tp.time_since_epoch();
        cout << round<minutes>(d) << endl;
    }
}

//////////////////////////////////////////
hours operator"" _h(unsigned long long n)
{
    return hours(n);
}

seconds operator"" _s(unsigned long long n)
{
    return seconds(n);
}

milliseconds operator"" _ms(unsigned long long n)
{
    return milliseconds(n);
}
void case5()
{
    auto h = 5_h;
    auto s = 45_s;
    auto ms = 200_ms;

    cout << h << s << ms << endl;
}
//////////////////////////////////////////
void case6()
{
    auto tp = system_clock::now();
    auto t = system_clock::to_time_t(tp);

    cout << std::ctime(&t) << endl;
}

//////////////////////////////////////////
class steady_timer final
{
private:
    typedef boost::chrono::steady_clock clock_type;

    //typedef clock_type::duration duration_type;
    typedef clock_type::time_point time_point_type;
    typedef boost::chrono::microseconds duration_type;

    time_point_type m_start = clock_type::now();
public:
    steady_timer() = default;
    ~steady_timer() = default;
public:
    void restart()
    {
        m_start = clock_type::now();
    }

    duration_type elapsed() const
    {
        return round<duration_type>(
                clock_type::now() - m_start);
    }
};

//////////////////////////////////////////

int main()
{
    steady_timer t;

    case1();
    case2();
    case3();
    case4();
    case5();
    case6();

    cout << t.elapsed() << endl;
}