/*
 * Copyright (c) 2016 Andreas Pohl
 * Licensed under MIT (see COPYING)
 *
 * Author: Andreas Pohl
 */

#include "hash.h"

#include <array>
#include <boost/utility/string_ref.hpp>
#include <functional>
#include <iomanip>
#include <memory>
#include <openssl/evp.h>
#include <sstream>
#include <vector>

namespace petrel {
namespace lib {

DECLARE_LIB_BEGIN(hash);
ADD_LIB_FUNCTION(md5);
ADD_LIB_FUNCTION(sha1);
ADD_LIB_FUNCTION(fnv);
DECLARE_LIB_BUILTIN_END();

using evp_hash_type = std::array<std::uint8_t, EVP_MAX_MD_SIZE>;

std::uint32_t evp_hash(lua_State* L, const EVP_MD* md, evp_hash_type& result, const char* data, std::size_t len) {
    std::unique_ptr<EVP_MD_CTX, std::function<void(EVP_MD_CTX*)>> md_ctx(EVP_MD_CTX_create(), EVP_MD_CTX_destroy);
    if (nullptr == md_ctx) {
        luaL_error(L, "failed to create md_ctx");
    }
    if (!EVP_DigestInit_ex(md_ctx.get(), md, 0)) {
        luaL_error(L, "failed to initialize digest");
    }
    if (!EVP_DigestUpdate(md_ctx.get(), data, len)) {
        luaL_error(L, "failed to update digest");
    }
    std::uint32_t size = 0;
    if (!EVP_DigestFinal_ex(md_ctx.get(), result.data(), &size)) {
        luaL_error(L, "failed to finalize digest");
    }
    return size;
}

void evp_pushhash(lua_State* L, evp_hash_type& h, std::uint32_t size) {
    std::ostringstream os;
    os << std::hex;
    for (std::uint32_t i = 0; i < size; ++i) {
        os << std::setw(2) << std::setfill('0') << static_cast<std::uint16_t>(h[i]);
    }
    lua_pushstring(L, os.str().c_str());
}

int hash::md5(lua_State* L) {
    boost::string_ref str(luaL_checkstring(L, 1));
    const EVP_MD* md = EVP_md5();
    if (nullptr == md) {
        luaL_error(L, "failed to get sha1 md");
    }
    evp_hash_type h;
    evp_pushhash(L, h, evp_hash(L, md, h, str.data(), str.size()));
    return 1;
}

int hash::sha1(lua_State* L) {
    boost::string_ref str(luaL_checkstring(L, 1));
    const EVP_MD* md = EVP_sha1();
    if (nullptr == md) {
        luaL_error(L, "failed to get sha1 md");
    }
    evp_hash_type h;
    evp_pushhash(L, h, evp_hash(L, md, h, str.data(), str.size()));
    return 1;
}

int hash::fnv(lua_State* L) {
    boost::string_ref str(luaL_checkstring(L, 1));
    auto* p = reinterpret_cast<const unsigned char*>(str.data());
    std::uint32_t h = 2166136261;
    for (std::size_t i = 0; i < str.size(); ++i) {
        h = (h * 16777619) ^ p[i];
    }
    lua_pushinteger(L, h);
    return 1;
}

}  // lib
}  // petrel


//
// Copyright (c) 2016 Heiko Bauke
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_EIGEN_VECTOR_HPP
#define BOOST_NUMERIC_BINDINGS_EIGEN_VECTOR_HPP

#include <boost/numeric/bindings/detail/adaptor.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/end.hpp>
#include <Eigen/Core>

namespace boost {
namespace numeric {
namespace bindings {
namespace detail {

template< typename T, int S, int Options, typename Id, typename Enable >
struct adaptor< Eigen::Matrix< T, S, 1, Options >, Id, Enable > {

    typedef typename copy_const< Id, T >::type value_type;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::entity, tag::vector >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::data_structure, tag::linear_array >,
        mpl::pair< tag::stride_type<1>, tag::contiguous >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.rows();
    }

    static value_type* begin_value( Id& id ) {
      return id.data();
    }

    static value_type* end_value( Id& id ) {
      return id.data() + id.size();
    }

};

template< typename T, int S, int Options, typename Id, typename Enable >
struct adaptor< Eigen::Matrix< T, 1, S, Options >, Id, Enable > {

    typedef typename copy_const< Id, T >::type value_type;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::entity, tag::vector >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::data_structure, tag::linear_array >,
        mpl::pair< tag::stride_type<1>, tag::contiguous >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.cols();
    }

    static value_type* begin_value( Id& id ) {
      return id.data();
    }

    static value_type* end_value( Id& id ) {
      return id.data() + id.size();
    }

};

template< typename T, int S, bool q, typename Id, typename Enable >
struct adaptor< Eigen::Block< T, 1, S, q >, Id, Enable > {

    typedef typename copy_const< Id, typename T::value_type >::type value_type;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::entity, tag::vector >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::data_structure, tag::linear_array >,
        mpl::pair< tag::stride_type<1>, std::ptrdiff_t >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.size();
    }

    static value_type* begin_value( Id& id ) {
      return id.data();
    }

    static value_type* end_value( Id& id ) {
      return id.data() + size1(id)*stride1(id);
    }

    static std::ptrdiff_t stride1( const Id& id ) {
      return id.colStride();
    }

};

template< typename T, int S, bool q, typename Id, typename Enable >
struct adaptor< Eigen::Block< T, S, 1, q >, Id, Enable > {

    typedef typename copy_const< Id, typename T::value_type >::type value_type;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::entity, tag::vector >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::data_structure, tag::linear_array >,
        mpl::pair< tag::stride_type<1>, std::ptrdiff_t >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.size();
    }

    static value_type* begin_value( Id& id ) {
      return id.data();
    }

    static value_type* end_value( Id& id ) {
      return id.data() + size1(id)*stride1(id);
    }

    static std::ptrdiff_t stride1( const Id& id ) {
      return id.rowStride();
    }

};

} // namespace detail
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include <iostream>
#include <vector>
#include <cstdlib>

#include <cmath>
#include <boost/thread.hpp>

void LongFunction();

int main(int argc, char* argv[])
{
  std::cout << "Main: Before thread call" << std::endl;

  boost::thread MyThread(&LongFunction);

  std::cout << "Main: After thread call" << std::endl;

  MyThread.join();

  return 0;
}

void LongFunction()
{
  std::cout << "Start LongFunction" << std::endl;
  unsigned int BigNum = 1e7;

  double temp;
  for(unsigned int i = 1; i < BigNum; i++)
  {
    temp = sin(i) / i;
    std::cout << "\r" << std::setw(10) << i << std::setw(10) << temp;
    std::cout.flush();
  }
  std::cout << "End LongFunction" << std::endl;
}


//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/locale.hpp>
#include <iostream>
#include <iomanip>
#include <ctime>

int main()
{
    using namespace boost::locale;

    generator gen;
    std::locale::global(gen(""));
    std::cout.imbue(std::locale());
    // Setup environment

    boost::locale::date_time now;

    date_time start=now;

    // Set the first day of the first month of this year
    start.set(period::month(),now.minimum(period::month()));
    start.set(period::day(),start.minimum(period::day()));

    int current_year = period::year(now);

    // Display current year
    std::cout << format("{1,ftime='%Y'}") % now << std::endl;

    //
    // Run forward untill current year is the date
    //
    for(now=start; period::year(now) == current_year;) {

        // Print heading of month
        if(calendar().is_gregorian())
            std::cout << format("{1,ftime='%B'}") % now <<std::endl;
        else
            std::cout << format("{1,ftime='%B'} ({1,ftime='%Y-%m-%d',locale=en} - {2,locale=en,ftime='%Y-%m-%d'})")
                % now
                % date_time(now,now.maximum(period::day())*period::day()) << std::endl;

        int first = calendar().first_day_of_week();

        // Print weeks days
        for(int i=0;i<7;i++) {
            date_time tmp(now,period::day_of_week() * (first + i));
            std::cout << format("{1,w=8,ftime='%a'} ") % tmp;
        }
        std::cout << std::endl;

        int current_month = now / period::month();
        int skip = now / period::day_of_week_local() - 1;
        for(int i=0;i<skip*9;i++)
            std::cout << ' ';
        for(;now / period::month() == current_month ;now += period::day()) {
            std::cout << format("{1,w=8,ftime='%e'} ") % now;
            if(now / period::day_of_week_local() == 7)
                std::cout << std::endl;
        }
        std::cout << std::endl;
    }

}
// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4


#include <boost/numeric/ublas/hermitian.hpp>


#include <boost/numeric/odeint/stepper/generation.hpp>


#ifdef WITH_LIBDAI
#include <boost/python.hpp>
#include <string>
#include "inf_def_visitor.hxx"

#include <opengm/inference/external/libdai/bp.hxx>
#include <opengm/inference/external/libdai/fractional_bp.hxx>
#include <opengm/inference/external/libdai/tree_reweighted_bp.hxx>
#include <opengm/inference/external/libdai/double_loop_generalized_bp.hxx>
#include <opengm/inference/external/libdai/junction_tree.hxx>
#include <opengm/inference/external/libdai/gibbs.hxx>
#include <opengm/inference/external/libdai/dec_map.hxx>
#include <param/libdai_param.hxx>

using namespace boost::python;

#if PY_MAJOR_VERSION == 2
static void wrap_import_array() {
    import_array();
}
#else
static void * wrap_import_array() {
    import_array();
}
#endif

template<class GM,class ACC>
void export_libdai_inference(){

	using namespace boost::python;
   wrap_import_array();
   append_subnamespace("solver");

   // INFERENCE TYPEDEFS
	typedef GM PyGm;
	typedef typename PyGm::ValueType ValueType;
	typedef typename PyGm::IndexType IndexType;
	typedef typename PyGm::LabelType LabelType;
   // bp
	typedef opengm::external::libdai::Bp<PyGm, ACC>                      PyLibdaiBp;
   //fractional bp
	typedef opengm::external::libdai::FractionalBp<PyGm, ACC>            PyLibdaiFractionalBp;
   // trbp
   typedef opengm::external::libdai::TreeReweightedBp<PyGm, ACC>        PyLibdaiTrbp;
   // junction tree
   typedef opengm::external::libdai::JunctionTree<PyGm, ACC>            PyLibdaiJunctionTree;
   // gibbs
   typedef opengm::external::libdai::Gibbs<PyGm, ACC>                   PyLibdaiGibbs;

   // dec map
   typedef opengm::external::libdai::DecMap<PyLibdaiBp>                 PyLibDaiDecMapBp;
   typedef opengm::external::libdai::DecMap<PyLibdaiBp>                 PyLibDaiDecMapFractionalBp;
   typedef opengm::external::libdai::DecMap<PyLibdaiBp>                 PyLibDaiDecMapTrwBp;
   //
   //typedef opengm::external::libdai::DoubleLoopGeneralizedBP<PyGm, ACC> PyLibDaiDoubleLoopGeneralizedBP;

   // setup
   InfSetup setup;
   std::string srName = semiRingName  <typename GM::OperatorType,ACC >() ;
   setup.dependencies = "This algorithm needs LibDai, compile OpenGM with CMake-Flag ``WITH_LIBDAI`` set to ``ON`` ";

   // bp
   {
      setup.algType    = "message-passing";
      setup.guarantees = "";
      typedef PyLibdaiBp LibDaiInf;
      const std::string enumName=std::string("_BpUpdateRuleLibDai")+srName;
      enum_<typename LibDaiInf::UpdateRule> (enumName.c_str())
      .value("parall", LibDaiInf::PARALL)
      .value("seqfix", LibDaiInf::SEQFIX)
      .value("seqrnd", LibDaiInf::SEQRND)
      .value("seqmax", LibDaiInf::SEQMAX)
      ;

      // export parameter
      exportInfParam<LibDaiInf>("_BeliefPropagationLibDai");
      // export inference
      class_< LibDaiInf>("_BeliefPropagationLibDai",init<const GM & >())
      .def(InfSuite<LibDaiInf,false>(std::string("BeliefPropagationLibDai"),setup))
      ;
   }
   // fractional bp
   {
      setup.algType    = "message-passing";
      setup.guarantees = "";
      typedef PyLibdaiFractionalBp LibDaiInf;
      const std::string enumName=std::string("_FractionalBpUpdateRuleLibDai")+srName;
      enum_<typename LibDaiInf::UpdateRule> (enumName.c_str())
      .value("parall", LibDaiInf::PARALL)
      .value("seqfix", LibDaiInf::SEQFIX)
      .value("seqrnd", LibDaiInf::SEQRND)
      .value("seqmax", LibDaiInf::SEQMAX)
      ;

      // export parameter
      exportInfParam<LibDaiInf>("_FractionalBpLibDai");
      // export inference
      class_< LibDaiInf>("_FractionalBpLibDai",init<const GM & >())
      .def(InfSuite<LibDaiInf,false>(std::string("FractionalBpLibDai"),setup))
      ;
   }
   // trw bp
   {
      setup.algType    = "message-passing";
      setup.guarantees = "";
      typedef PyLibdaiTrbp LibDaiInf;
      const std::string enumName=std::string("_TreeReweightedBpUpdateRuleLibDai")+srName;
      enum_<typename LibDaiInf::UpdateRule> (enumName.c_str())
      .value("parall", LibDaiInf::PARALL)
      .value("seqfix", LibDaiInf::SEQFIX)
      .value("seqrnd", LibDaiInf::SEQRND)
      .value("seqmax", LibDaiInf::SEQMAX)
      ;

      // export parameter
      exportInfParam<LibDaiInf>("_TreeReweightedBpLibDai");
      // export inference
      class_< LibDaiInf>("_TreeReweightedBpLibDai",init<const GM & >())
      .def(InfSuite<LibDaiInf,false>(std::string("TreeReweightedBpLibDai"),setup))
      ;
   }
   // junction tree
   {
      setup.algType    = "dynamic-programming";
      setup.guarantees = "global optimal";
      typedef PyLibdaiJunctionTree LibDaiInf;

      const std::string enumName1=std::string("_JunctionTreeUpdateRuleLibDai")+srName;
      enum_<typename LibDaiInf::UpdateRule> (enumName1.c_str())
      .value("hugin", LibDaiInf::HUGIN)
      .value("shsh", LibDaiInf::SHSH)
      ;
      const std::string enumName2=std::string("_JunctionTreeHeuristicLibDai" )+srName;
      enum_<typename LibDaiInf::Heuristic> (enumName2.c_str())
      .value("minfill", LibDaiInf::MINFILL)
      .value("weightedminfill", LibDaiInf::WEIGHTEDMINFILL)
      .value("minweight", LibDaiInf::MINWEIGHT)
      .value("minneighbors", LibDaiInf::MINNEIGHBORS)
      ;

      // export parameter
      exportInfParam<LibDaiInf>("_JunctionTreeLibDai");
      // export inference
      class_< LibDaiInf>("_JunctionTreeLibDai",init<const GM & >())
      .def(InfSuite<LibDaiInf,false>(std::string("JunctionTreeLibDai"),setup))
      ;
   }
   // gibbs
   {
      setup.algType    = "sampling";
      setup.guarantees = "";
      typedef PyLibdaiGibbs LibDaiInf;

      // export parameter
      exportInfParam<LibDaiInf>("_GibbsLibDai");
      // export inference
      class_< LibDaiInf>("GibbsLibDai",init<const GM & >())
      .def(InfSuite<LibDaiInf,false>(std::string("GibbsLibDai"),setup))
      ;
   }
   // decmap
   {
      setup.algType    ="decimation-algorithm";
      setup.cite       = "";
      setup.guarantees = "";
      setup.notes      ="Approximate inference algorithm DecMAP, which constructs a MAP state by decimation.\n\n"
                        "Decimation involves repeating the following two steps until no free variables remain:\n\n"
                        "Run an approximate inference algorithm,clamp the factor with the lowest entropy to its most probable state\n\n";
      setup.hyperParameterKeyWords        = StringVector(1,std::string("subInference"));
      setup.hyperParametersDoc            = StringVector(1,std::string("sub-inference algorithms of dec-map"));
      // parameter of inference will change if hyper parameter changes
      setup.hasInterchangeableParameter   = false;
      // decmap-bp
      {
         typedef opengm::external::libdai::DecMap<PyLibdaiBp>  LibDaiInf;
         setup.isDefault=true;
         setup.hyperParameters= StringVector(1,std::string("bp"));
         // export parameter
         exportInfParam<LibDaiInf>("_DecimationBpLibDai");
         // export inference
         class_< LibDaiInf>("_DecimationBpLibDai",init<const GM & >())
         .def(InfSuite<LibDaiInf,false>(std::string("DecimationLibDai"),setup))
         ;
      }
      // decmap-fractionalBp
      {
         typedef opengm::external::libdai::DecMap<PyLibdaiFractionalBp>  LibDaiInf;
         setup.isDefault=false;
         setup.hyperParameters= StringVector(1,std::string("fractionalBp"));
         // export parameter
         exportInfParam<LibDaiInf>("_DecimationFractionalBpLibDai");
         // export inference
         class_< LibDaiInf>("_DecimationFractionalBpLibDai",init<const GM & >())
         .def(InfSuite<LibDaiInf,false>(std::string("DecimationLibDai"),setup))
         ;
      }

      // decmap-trwbp
      {
         typedef opengm::external::libdai::DecMap<PyLibdaiTrbp>  LibDaiInf;
         setup.isDefault=false;
         setup.hyperParameters= StringVector(1,std::string("trwBp"));
         // export parameter
         exportInfParam<LibDaiInf>("_DecimationTrwBpLibDai");
         // export inference
         class_< LibDaiInf>("_DecimationTrwBpLibDai",init<const GM & >())
         .def(InfSuite<LibDaiInf,false>(std::string("DecimationLibDai"),setup))
         ;
      }
      // decmap-trwbp
      {
         typedef opengm::external::libdai::DecMap<PyLibdaiGibbs>  LibDaiInf;
         setup.isDefault=false;
         setup.hyperParameters= StringVector(1,std::string("gibbs"));
         // export parameter
         exportInfParam<LibDaiInf>("_DecimationGibbsLibDai");
         // export inference
         class_< LibDaiInf>("_DecimationGibbsLibDai",init<const GM & >())
         .def(InfSuite<LibDaiInf,false>(std::string("DecimationLibDai"),setup))
         ;
      }
   }

   // throws Feature not implemented Feature not implemented
   /*
   // double loop generalized bp
   {
      setup.algType    = "message-passing";
      setup.guarantees = "";
      typedef PyLibDaiDoubleLoopGeneralizedBP LibDaiInf;

      const std::string enumName1=std::string("_DoubleLoopGenereralizedBpInitLibDai")+srName;
      enum_<typename LibDaiInf::Init> (enumName1.c_str())
         .value("uniform", LibDaiInf::UNIFORM)
         .value("random", LibDaiInf::RANDOM)
      ;
      const std::string enumName2=std::string("_DoubleLoopGenereralizedBpClustersLibDai")+srName;
      enum_<typename LibDaiInf::Clusters> (enumName2.c_str())
         .value("min", LibDaiInf::MIN)
         .value("bethe", LibDaiInf::BETHE)
         .value("delta", LibDaiInf::DELTA)
         .value("loop", LibDaiInf::LOOP)
      ;

      // export parameter
      exportInfParam<LibDaiInf>("_DoubleLoopGenereralizedBpLibDai");
      // export inference
      class_< LibDaiInf>("DoubleLoopGenereralizedBpLibDai",init<const GM & >())
      .def(InfSuite<LibDaiInf,false>(std::string("DoubleLoopGenereralizedBpLibDai"),setup))
      ;
   }
   */
}

template void export_libdai_inference<opengm::python::GmAdder, opengm::Minimizer>();
//template void export_libdai_inference<GmAdder, opengm::Maximizer>();
//template void export_libdai_inference<GmMultiplier, opengm::Minimizer>();
//template void export_libdai_inference<GmMultiplier, opengm::Maximizer>();

#endif


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <iostream>
#include <limits>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/utilityMacros.h"
#include "Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h"

namespace tudat
{
namespace unit_tests
{

//! Test suit for numerical integrators.
BOOST_AUTO_TEST_SUITE( test_numerical_integrator )

//! Using declaration of the NumericalIntegrator.
using numerical_integrators::NumericalIntegrator;

//! State derivative function that always returns zero.
/*!
 * State derivative function that always returns a zero vector with length equal to the input.
 * \param time Time at which the state derivative needs to be evaluated.
 * \param state State at which the state derivative needs to be evalated.
 * \return Zero vector with length equal to state.
 */
Eigen::VectorXd computeZeroStateDerivative( const double time,
                                            const Eigen::VectorXd& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    return Eigen::VectorXd::Zero( state.rows( ) );
}

//! Dummy numerical integrator.
/*!
 * Dummy numerical integrator that keeps track of the amount of steps made.
 */
class DummyNumericalIntegrator :
        public NumericalIntegrator< double, Eigen::VectorXd, Eigen::VectorXd >
{
public:

    //! Default constructor.
    /*!
     * Default constructor, setting zeroDerivative as the state derivative function.
     */
    DummyNumericalIntegrator( const double intervalStart, const Eigen::VectorXd& initialState )
        : NumericalIntegrator< >( &computeZeroStateDerivative ),
          numberOfSteps( 0 ),
          currentIndependentVariable_( intervalStart ),
          currentState_( initialState )
    { }

    //! Get step size of the next step.
    /*!
     * Returns the step size of the next step.
     * \return Step size to be used for the next step.
     */
    virtual double getNextStepSize( ) const { return stepSize_; }

    //! Get current state.
    /*!
     * Returns the current state stored interally by the integrator.
     * \return Current state.
     */
    virtual Eigen::VectorXd getCurrentState( ) const { return currentState_; }

    //! Get current independent variable.
    /*!
     * Returns the current value of the independent variable stored interally by the integrator.
     * \return Current independent variable.
     */
    virtual double getCurrentIndependentVariable( ) const { return currentIndependentVariable_; }

    //! Rollback the internal state to the last state.
    /*!
     * Performs rollback of the internal state to the last state. This function can only be called
     * once after calling integrateTo( ) or performIntegrationStep( ) unless specified otherwise by
     * implementations, and can not be called before any of these functions have been called. Will
     * return true if the rollback was succesful, and false otherwise.
     * \return True if the rollback was successful.
     */
    virtual bool rollbackToPreviousState( ) { return true; }

    //! Perform a single integration step that does not do anything.
    /*!
     * Performs a single integration step that does not do anything, except incrementing the
     * step counter.
     * \param stepSize The step size of this step.
     * \return The state history at the end of the interval, which is basically equal to the input.
     */
    virtual Eigen::VectorXd performIntegrationStep( const double stepSize )
    {
        // Increment the step counter.
        numberOfSteps++;

        stepSize_ = stepSize;
        currentIndependentVariable_ += stepSize_;

        return currentState_;
    }

    //! Counter of the number of steps.
    /*!
     * Counter of the number of steps taken.
     */
    int numberOfSteps;

protected:

    //! Last used step size by this integrator.
    /*!
     * Last used step size by this integrator.
     */
    double stepSize_;

    //! Current independent variable value.
    /*!
     * Current value of independent variable stored internally by the integrator.
     */
    double currentIndependentVariable_;

    //! Current state.
    /*!
     * Current state stored internally by the integrator.
     */
    Eigen::VectorXd currentState_;
};

//! Test the amount of steps that NumericalIntegrator::integrateTo takes.
/*!
 * Test the amount of steps that NumericalIntegrator::integrateTo takes.
 * \param intervalStart The start of the integration interval.
 * \param intervalEnd The end of the integration interval.
 * \param initialState The initial state.
 * \param stepSize The step size to take.
 * \return True if actual number of steps is equal to actual number of steps; false otherwise.
 */
bool testIntegrateToFunction( const double intervalStart, const double intervalEnd,
                              const Eigen::VectorXd& initialState,
                              const double stepSize )
{
    DummyNumericalIntegrator integrator( intervalStart, initialState );
    Eigen::VectorXd integratedState = integrator.integrateTo( intervalEnd, stepSize );

    // Calculate expected number of steps.
    int expectedNumberOfSteps = static_cast< int >(
                std::ceil( ( intervalEnd - intervalStart ) / stepSize ) );

    // Check if the integrated state is equal to the initial state.
    // This is an exact comparison, because no arithmetics are performed on the state.
    if ( integratedState != initialState )
    {
        std::cerr << "DummyNumericalIntegrator was not a dummy integrator!" << std::endl;
        return false;
    }

    // Check if the actual number of steps is equal to the expected number of steps.
    if ( integrator.numberOfSteps != expectedNumberOfSteps )
    {
        return false;
    }

    return true;
}

//! Test if the numerical integrator base class is working correctly.
BOOST_AUTO_TEST_CASE( testNumberOfStepsUsingNumericalIntegrator )
{
    // Set random initial state.
    Eigen::VectorXd initialState = ( Eigen::VectorXd( 4 ) << 0.34, 0.24, 0.76, 0.10 ).finished( );

    // Case 1: test number of steps for different start and end times.
    {
        BOOST_CHECK( testIntegrateToFunction( 0.0, 0.0, initialState, 10.0 ) );
        BOOST_CHECK( testIntegrateToFunction( 0.0, 10.0, initialState, 10.0 ) );
        BOOST_CHECK( testIntegrateToFunction( 0.0, 20.0, initialState, 10.0 ) );
        BOOST_CHECK( testIntegrateToFunction( 0.0, 30.0, initialState, 10.0 ) );
    }

    // Case 2: test number of steps for different step sizes.
    {
        BOOST_CHECK( testIntegrateToFunction( 0.0, 10.0, initialState, 2.5 ) );
        BOOST_CHECK( testIntegrateToFunction( 0.0, 10.0, initialState, 3.0 ) );
        BOOST_CHECK( testIntegrateToFunction( 0.0, 10.0, initialState, 3.5 ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright 2021 Apex.AI, Inc.
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

/// \copyright Copyright 2021 Apex.AI, Inc.
/// All rights reserved.

#ifndef STATE_ESTIMATION_NODES__HISTORY_HPP_
#define STATE_ESTIMATION_NODES__HISTORY_HPP_

#include <common/types.hpp>
#include <helper_functions/mahalanobis_distance.hpp>
#include <mpark_variant_vendor/variant.hpp>
#include <state_estimation_nodes/steady_time_grid.hpp>

#include <Eigen/Cholesky>
#include <Eigen/Core>

#include <chrono>
#include <map>
#include <memory>
#include <utility>

namespace autoware
{
namespace common
{
namespace state_estimation
{

namespace detail
{

///
/// Check if the sample passes the Mahalanobis gate.
///
/// @param[in]  sample                     The input sample
/// @param[in]  mean                       The mean to be compared against
/// @param[in]  covariance                 Covariance around the given mean
/// @param[in]  mahalanobis_threshold      The mahalanobis threshold factor
///
/// @tparam     kNumOfStates               Number of states in the state vector.
///
/// @return     True if the sample passes the gate and false otherwise.
///
template<std::int32_t kNumOfStates>
bool passes_mahalanobis_gate(
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, 1> & sample,
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, 1> & mean,
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, kNumOfStates> & covariance,
  const common::types::float32_t mahalanobis_threshold)
{
  using Matrix = Eigen::Matrix<common::types::float32_t, kNumOfStates, kNumOfStates>;
  const Matrix L = covariance.llt().matrixL();
  const auto squared_mahalanobis_distance =
    autoware::common::helper_functions::calculate_squared_mahalanobis_distance(sample, mean, L);
  const auto squared_threshold = mahalanobis_threshold * mahalanobis_threshold;
  return squared_mahalanobis_distance < squared_threshold;
}

}  // namespace detail

/// @brief      An event to indicate a prediction step.
struct PredictionEvent {};

///
/// @brief      An event to reset the state of the filter.
///
/// @tparam     FilterT  Type of the EKF filter used to infer vector and matrix types.
///
template<typename FilterT>
struct ResetEvent
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typename FilterT::State state;
  typename FilterT::State::Matrix covariance;
};

///
/// @brief      This class encapsulates a history of events used with EKF.
///
///             The class handles adding events to a history of a specified size. It models the
///             behavior of a circular buffer, meaning that as new events come in, the oldest ones
///             are removed. The events can be either measurement types or specific events like
///             reset or prediction. Whenever an event is added to the middle of the history all the
///             following events get rolled on top of this event to produce a new state.
///
/// @tparam     FilterT       Type of EKF filter used.
/// @tparam     kNumOfStates  Dimensionality of the state in the filter.
/// @tparam     EventT        A variadic template of all possible events.
///
template<typename FilterT, typename ... EventT>
class History
{
  ///
  /// @brief      A single entry in the history. Holds state and covariance and a variant of events.
  ///
  class HistoryEntry;
  ///
  /// @brief      A functor that handles updating the state. It is used with the variant stored
  ///             within the HistoryEntry.
  ///
  class EkfStateUpdater;

  /// Typedef for timestamps.
  using Timestamp = std::chrono::system_clock::time_point;
  /// Typedef for history map type.
  using HistoryMap = std::multimap<Timestamp, HistoryEntry>;

public:
  ///
  /// @brief      Construct history from a filter pointer with a specific size.
  ///
  /// @param      filter                 The filter pointer to be used internally.
  /// @param[in]  max_history_size       The maximum history size.
  /// @param[in]  mahalanobis_threshold  The mahalanobis threshold
  ///
  explicit History(
    FilterT & filter,
    const std::size_t max_history_size,
    const common::types::float32_t mahalanobis_threshold)
  : m_filter{filter},
    m_max_history_size{max_history_size},
    m_mahalanobis_threshold{mahalanobis_threshold} {}

  ///
  /// @brief      Add an event to history. If it is added to the middle the following ones are
  ///             automatically replayed on top of it.
  ///
  /// @param[in]  timestamp  The timestamp of the event.
  /// @param[in]  entry      The entry to be added to history.
  ///
  void emplace_event(const Timestamp & timestamp, const HistoryEntry & entry);
  /// @brief      Check if the history is empty.
  inline bool empty() const noexcept {return m_history.empty();}
  /// @brief      Get size of history.
  inline std::size_t size() const noexcept {return m_history.size();}
  /// @brief      Get last timestamp in history.
  inline const Timestamp & get_last_timestamp() const noexcept {return m_history.rbegin()->first;}
  /// @brief      Get last event in history.
  inline const HistoryEntry & get_last_event() const noexcept {return m_history.rbegin()->second;}
  /// @brief      Get the filter as a const ref.
  const FilterT & get_filter() const noexcept {return m_filter;}
  /// @brief      Get the filter.
  FilterT & get_filter() noexcept {return m_filter;}

private:
  ///
  /// @brief      If the history is too large, drop the oldest event from it.
  ///
  inline void drop_oldest_event_if_needed()
  {
    if ((m_history.size() >= m_max_history_size) && (m_max_history_size > 0U)) {
      (void) m_history.erase(m_history.begin());
    }
  }

  ///
  /// @brief      Update all the following events as their state is based on the current one.
  ///
  /// @param[in]  start_iter  The current iterator with the new state.
  ///
  void update_impacted_events(const typename HistoryMap::iterator & start_iter);

  HistoryMap m_history{};  ///< history of events.
  FilterT & m_filter{};  ///< pointer to the filter implementation.
  std::size_t m_max_history_size{};  ///< Maximum number of events in history.
  common::types::float32_t m_mahalanobis_threshold{};  ///< Mahalanobis distance threshold.
};

template<typename FilterT, typename ... EventT>
class History<FilterT, EventT...>::HistoryEntry
{
public:
  // cppcheck-suppress unknownMacro  // cppcheck seems to be confused due to lots of templates.
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  template<typename SingleEventT>
  // cppcheck-suppress noExplicitConstructor; Conversion to the variant type takes place.
  HistoryEntry(const SingleEventT & event) : m_event {event} {}

  /// @brief      Update the stored state.
  void update_stored_state(const typename FilterT::State & state) noexcept
  {
    m_stored_state = state;
  }
  /// @brief      Get the stored state.
  const typename FilterT::State & stored_state() const noexcept {return m_stored_state;}
  /// @brief      Set the stored covariance.
  void update_stored_covariance(const typename FilterT::State::Matrix & covariance) noexcept
  {
    m_stored_covariance = covariance;
  }
  /// @brief      Get the stored covariance.
  const typename FilterT::State::Matrix & stored_covariance() const noexcept
  {
    return m_stored_covariance;
  }
  /// @brief      Get the event stored in this history entry.
  const mpark::variant<EventT...> & event() const {return m_event;}

private:
  /// State stored in this entry.
  typename FilterT::State m_stored_state{};
  /// Covariance stored in this entry.
  typename FilterT::State::Matrix m_stored_covariance{FilterT::State::Matrix::Zero()};
  /// Event stored in this history entry.
  mpark::variant<EventT...> m_event;
};

template<typename FilterT, typename ... EventT>
class History<FilterT, EventT...>::EkfStateUpdater
{
public:
  explicit EkfStateUpdater(
    FilterT & filter,
    const common::types::float32_t mahalanobis_threshold,
    const std::chrono::system_clock::duration & dt = std::chrono::milliseconds{0})
    : m_filter {filter}, m_mahalanobis_threshold{mahalanobis_threshold}, m_dt{dt}
  {}

  ///
  /// @brief      An operator that passes a measurement event to the filter implementation.
  ///
  /// @param[in]  event         A measurement event.
  ///
  /// @tparam     MeasurementT  Type of measurement event. Can be any measurement type.
  ///
  template<typename MeasurementT>
  void operator()(const MeasurementT & event)
  {
    m_filter.predict(m_dt);
    // TODO(#887): I see a couple of ways to check mahalanobis distance in case the measurement does
    // not cover the full state. Here I upscale it to the full state, copying the values of the
    // current state for ones missing in the observation. We can alternatively apply the H matrix
    // and only compute the distance in the measurement world. Don't really know which one is best
    // here.
    const auto measurement_as_state = event.map_into(m_filter.state());
    if (!detail::passes_mahalanobis_gate(
        measurement_as_state.vector(),
        m_filter.state().vector(),
        m_filter.covariance(),
        m_mahalanobis_threshold)) {return;}
    m_filter.correct(event);
  }

  /// @brief      An operator that resets the state of the filter implementation.
  void operator()(const ResetEvent<FilterT> & event)
  {
    m_filter.reset(event.state, event.covariance);
  }

  /// @brief      An operator that applies the prediction event to the filter implementation.
  void operator()(const PredictionEvent &)
  {
    m_filter.predict(m_dt);
  }

private:
  FilterT & m_filter{};  ///< A pointer to the filter implementation.
  common::types::float32_t m_mahalanobis_threshold{};  ///< Mahalanobis distance threshold.
  std::chrono::system_clock::duration m_dt{};  ///< Current time step.
};

template<typename FilterT, typename ... EventT>
void History<FilterT, EventT...>::emplace_event(
  const Timestamp & timestamp, const HistoryEntry & entry)
{
  drop_oldest_event_if_needed();
  const auto iterator_to_inserted_position = m_history.emplace(timestamp, entry);
  update_impacted_events(iterator_to_inserted_position);
}

template<typename FilterT, typename ... EventT>
void History<FilterT, EventT...>::update_impacted_events(
  const typename HistoryMap::iterator & start_iter)
{
  Timestamp previous_timestamp{};
  if (start_iter == m_history.begin()) {
    if (!mpark::holds_alternative<ResetEvent<FilterT>>(start_iter->second.event())) {
      (void) m_history.erase(start_iter);
      throw std::runtime_error(
              "Non-reset event inserted to the beginning of history. This might "
              "happen if a very old event is inserted into the queue. Consider "
              "increasing the queue size or debug program latencies.");
    }
  } else {
    const auto prev_iter = std::prev(start_iter);
    previous_timestamp = prev_iter->first;
    const auto & prev_entry = prev_iter->second;
    m_filter.reset(
      typename FilterT::State{prev_entry.stored_state()},
      prev_entry.stored_covariance());
  }
  for (auto iter = start_iter; iter != m_history.end(); ++iter) {
    const auto current_timestamp = iter->first;
    auto & entry = iter->second;
    mpark::visit(
      EkfStateUpdater{m_filter, m_mahalanobis_threshold, current_timestamp - previous_timestamp},
      entry.event());
    entry.update_stored_state(m_filter.state());
    entry.update_stored_covariance(m_filter.covariance());
    previous_timestamp = iter->first;
  }
}

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION_NODES__HISTORY_HPP_


/*
 * pixel_to_miller_index.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/spot_prediction/pixel_to_miller_index.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;

  static vec3<double> PixelToMillerIndex_h_rotation(const PixelToMillerIndex &self,
                                                    std::size_t panel,
                                                    double x,
                                                    double y,
                                                    double z) {
    return self.h(panel, x, y, z);
  }

  static vec3<double> PixelToMillerIndex_h_stills(const PixelToMillerIndex &self,
                                                  std::size_t panel,
                                                  double x,
                                                  double y) {
    return self.h(panel, x, y);
  }

  void export_pixel_to_miller_index() {
    class_<PixelToMillerIndex>("PixelToMillerIndex", no_init)
      .def(init<const MonoBeam &,
                const Detector &,
                const Goniometer &,
                const Scan &,
                const CrystalBase &>())
      .def(init<const MonoBeam &, const Detector &, const CrystalBase &>())
      .def("h", &PixelToMillerIndex_h_rotation)
      .def("h", &PixelToMillerIndex_h_stills)
      .def("q", &PixelToMillerIndex::q);
  }

}}}  // namespace dials::algorithms::boost_python


#include "schunk_wsg_ros.h"

#include <vector>

#include <Eigen/Dense>

#include "drake/common/drake_assert.h"
#include "drake/common/eigen_types.h"
#include "drake/common/trajectories/piecewise_polynomial_trajectory.h"

namespace spartan {
namespace rlg_simulation {
namespace iiwa_rlg_simulation {

using namespace drake;
using systems::BasicVector;
using systems::Context;
using systems::DiscreteValues;
using drake::manipulation::schunk_wsg::SchunkWsgTrajectoryGeneratorStateVector;

SchunkWsgRosTrajectoryGenerator::SchunkWsgRosTrajectoryGenerator(
    int input_size, int position_index)
    : position_index_(position_index),
      target_output_port_(
          this->DeclareVectorOutputPort(
                  BasicVector<double>(2),
                  &SchunkWsgRosTrajectoryGenerator::OutputTarget)
              .get_index()),
      max_force_output_port_(
          this->DeclareVectorOutputPort(
                  BasicVector<double>(1),
                  &SchunkWsgRosTrajectoryGenerator::OutputForce)
              .get_index()) {
  this->set_name("SchunkWsgRosTrajectoryGenerator");
  this->DeclareAbstractInputPort();
  this->DeclareInputPort(systems::kVectorValued, input_size);
  // The update period below matches the polling rate from
  // drake-schunk-driver.
  this->DeclarePeriodicDiscreteUpdate(0.05);
}

void SchunkWsgRosTrajectoryGenerator::OutputTarget(
    const Context<double>& context, BasicVector<double>* output) const {
  const SchunkWsgTrajectoryGeneratorStateVector<double>* traj_state =
      dynamic_cast<const SchunkWsgTrajectoryGeneratorStateVector<double>*>(
          &context.get_discrete_state(0));

  if (trajectory_) {
    output->get_mutable_value() = trajectory_->value(
        context.get_time() - traj_state->trajectory_start_time());
  } else {
    output->get_mutable_value() =
        Eigen::Vector2d(traj_state->last_position(), 0);
  }
}

void SchunkWsgRosTrajectoryGenerator::OutputForce(
    const Context<double>& context, BasicVector<double>* output) const {
  const SchunkWsgTrajectoryGeneratorStateVector<double>* traj_state =
      dynamic_cast<const SchunkWsgTrajectoryGeneratorStateVector<double>*>(
          &context.get_discrete_state(0));
  output->get_mutable_value() = Vector1d(traj_state->max_force());
}

void SchunkWsgRosTrajectoryGenerator::DoCalcDiscreteVariableUpdates(
    const Context<double>& context,
    const std::vector<const systems::DiscreteUpdateEvent<double>*>&,
    DiscreteValues<double>* discrete_state) const {
  const systems::AbstractValue* input = this->EvalAbstractInput(context, 0);
  DRAKE_ASSERT(input != nullptr);
  const auto& command = input->GetValue<wsg50_msgs::WSG_50_command>();
  // The target_position_mm field represents the distance between
  // the two fingers. The fingers are connected by a mechanical
  // linkage, so the relative movement between the two fingers is
  // twice the actuator's movement (and what we want to calcuate
  // here is the value for the actuator).
  double target_position = -(command.position_mm / 1e3) / 2.;
  if (std::isnan(target_position)) {
    target_position = 0;
  }

  const systems::BasicVector<double>* state = this->EvalVectorInput(context, 1);
  const double cur_position = state->GetAtIndex(position_index_);

  const SchunkWsgTrajectoryGeneratorStateVector<double>* last_traj_state =
      dynamic_cast<const SchunkWsgTrajectoryGeneratorStateVector<double>*>(
          &context.get_discrete_state(0));
  SchunkWsgTrajectoryGeneratorStateVector<double>* new_traj_state =
      dynamic_cast<SchunkWsgTrajectoryGeneratorStateVector<double>*>(
          &discrete_state->get_mutable_vector(0));
  new_traj_state->set_last_position(cur_position);

  double max_force = command.force;
  if (std::isnan(max_force)) {
    max_force = 0;
  }
  new_traj_state->set_max_force(max_force);

  if (std::abs(last_traj_state->last_target_position() - target_position) >
      kTargetEpsilon) {
    UpdateTrajectory(cur_position, target_position);
    new_traj_state->set_last_target_position(target_position);
    new_traj_state->set_trajectory_start_time(context.get_time());
  } else {
    new_traj_state->set_last_target_position(
        last_traj_state->last_target_position());
    new_traj_state->set_trajectory_start_time(
        last_traj_state->trajectory_start_time());
  }
}

std::unique_ptr<DiscreteValues<double>>
SchunkWsgRosTrajectoryGenerator::AllocateDiscreteState() const {
  return std::make_unique<DiscreteValues<double>>(
      std::make_unique<SchunkWsgTrajectoryGeneratorStateVector<double>>());
}

void SchunkWsgRosTrajectoryGenerator::UpdateTrajectory(
    double cur_position, double target_position) const {
  // The acceleration and velocity limits correspond to the maximum
  // values available for manual control through the gripper's web
  // interface.
  const double kMaxVelocity = 0.42;  // m/s
  const double kMaxAccel = 5.;       // m/s^2
  const double kTimeToMaxVelocity = kMaxVelocity / kMaxAccel;
  // TODO(sam.creasey) this should probably consider current speed
  // if the gripper is already moving.
  const double kDistanceToMaxVelocity =
      0.5 * kMaxAccel * kTimeToMaxVelocity * kTimeToMaxVelocity;

  std::vector<Eigen::MatrixXd> knots;
  std::vector<double> times;
  knots.push_back(Eigen::Vector2d(cur_position, 0));
  times.push_back(0);

  const double direction = (cur_position < target_position) ? 1 : -1;
  const double delta = std::abs(target_position - cur_position);

  // The trajectory creation code below is, to say the best, a bit
  // primitive.  I (sam.creasey) would not be surprised if it could
  // be significantly improved.  It's also based only on the
  // configurable constants for the WSG 50, not on analysis of the
  // actual motion of the gripper.
  if (delta < kDistanceToMaxVelocity * 2) {
    // If we can't accelerate to our maximum (and decelerate again)
    // within the target travel distance, calculate the peak velocity
    // we will reach and create a trajectory which ramps to that
    // velocity and back down.
    const double mid_distance = delta / 2;
    const double mid_velocity =
        kMaxVelocity * (mid_distance / kDistanceToMaxVelocity);
    const double mid_time = mid_velocity / kMaxAccel;
    knots.push_back(Eigen::Vector2d(cur_position + mid_distance * direction,
                                    mid_velocity * direction));
    times.push_back(mid_time);
    knots.push_back(Eigen::Vector2d(target_position, 0));
    times.push_back(mid_time * 2);
  } else {
    knots.push_back(
        Eigen::Vector2d(cur_position + (kDistanceToMaxVelocity * direction),
                        kMaxVelocity * direction));
    times.push_back(kTimeToMaxVelocity);

    const double time_at_max =
        (delta - 2 * kDistanceToMaxVelocity) / kMaxVelocity;
    knots.push_back(
        Eigen::Vector2d(target_position - (kDistanceToMaxVelocity * direction),
                        kMaxVelocity * direction));
    times.push_back(kTimeToMaxVelocity + time_at_max);

    knots.push_back(Eigen::Vector2d(target_position, 0));
    times.push_back(kTimeToMaxVelocity + time_at_max + kTimeToMaxVelocity);
  }
  trajectory_.reset(new PiecewisePolynomialTrajectory(
      PiecewisePolynomial<double>::FirstOrderHold(times, knots)));
}

SchunkWsgRosStatusSender::SchunkWsgRosStatusSender(int input_size,
                                                   int position_index,
                                                   int velocity_index)
    : position_index_(position_index),
      velocity_index_(velocity_index) {
  this->set_name("SchunkWsgRosStatusSender");
  this->DeclareInputPort(systems::kVectorValued, input_size);
  this->DeclareAbstractOutputPort(&SchunkWsgRosStatusSender::OutputStatus);
}

void SchunkWsgRosStatusSender::OutputStatus(
    const Context<double>& context, wsg50_msgs::WSG_50_state* output) const {
  wsg50_msgs::WSG_50_state& status = *output;

  status.header.seq = 0;
  status.header.stamp = ros::Time::now();

  const systems::BasicVector<double>* state = this->EvalVectorInput(context, 0);

  // The position and speed reported in this message are between the
  // two fingers rather than the position/speed of a single finger
  // (so effectively doubled).
  status.position_mm = -2 * state->GetAtIndex(position_index_) * 1e3;
  // TODO(sam.creasey) Figure out how to get the actual force from
  // the plant so that we can populate this field.
  status.force = 0;
  status.speed_mm_per_s = -2 * state->GetAtIndex(velocity_index_) * 1e3;
}

}  // namespace schunk_wsg
}  // namespace manipulation
}  // namespace drake


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/** @file
 * @author   Collin Johnson
 *
 * social_norm_results is a program to pull information from one or more MPEPC logs and produce plots and figures that
 * evaluate the success of the social norms planning vs. regular MPEPC.
 *
 * social_norm_results requires specifying a file with the logs to process -- one per line -- format:
 *
 *   'social'/'regular' log_name local_topo_map
 *
 * The produced results are:
 *
 *   - Plot of the histogram over normalized dist for path travel
 *   - Plot of the histogram over normalized dist for area transitions
 *   - File containing:
 *       - stats for path interactions
 *       - stats for place interactions
 *       - stats for overall interactions
 *
 * Results are produced for social vs. regular logs.
 */

#include "core/angle_functions.h"
#include "hssh/local_topological/areas/serialization.h"
#include "hssh/local_topological/local_topo_map.h"
#include "math/t_test.h"
#include "math/z_test.h"
#include "mpepc/evaluation/interaction.h"
#include "mpepc/evaluation/mpepc_log.h"
#include "mpepc/evaluation/social_force.h"
#include "mpepc/evaluation/utils.h"
#include "mpepc/social/social_norm_utils.h"
#include "mpepc/social/topo_situation.h"
#include "mpepc/training/agent_state.h"
#include "utils/command_line.h"
#include "utils/histogram.h"
#include "utils/histogram_2d.h"
#include "utils/serialized_file_io.h"
#include <algorithm>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/as_array.hpp>
#include <boost/tuple/tuple.hpp>
#include <cstdlib>
#include <gnuplot-iostream.h>
#include <iostream>

using namespace vulcan;
using namespace vulcan::mpepc;

using namespace boost::accumulators;
using StatsAcc = accumulator_set<double, stats<tag::min, tag::max, tag::mean, tag::variance, tag::median>>;

const std::string kDistArg("max-dist");
const std::string kConeArg("cone-angle");
const std::string kLogsArg("logs");
const std::string kTrainArg("train");

const int kNumHistBins = 20;

struct StatResults
{
    std::string name;

    double min;
    double max;
    double mean;
    double stdDev;
    double median;
};

struct SituationResults
{
    TopoSituationResponse situation;
    StatsAcc forceAcc;
    StatsAcc blameAcc;
    StatsAcc distPassingAcc;
    StatsAcc speedPassingAcc;

    int leftPassCount = 0;
    int rightPassCount = 0;
};

struct VersionResults
{
    utils::Histogram pathDist;
    utils::Histogram gwyDist;

    utils::Histogram2D pathInteractions;

    utils::Histogram pathDistAgent;
    utils::Histogram gwyDistAgent;

    std::vector<SituationResults> situations;

    SituationResults pathResults;
    SituationResults oncomingResults;
    SituationResults overtakingResults;
    SituationResults beingPassedResults;

    std::string basename;
    std::string version;

    VersionResults(void)
    : pathDist(0.0, 1.0, kNumHistBins)
    , gwyDist(0.0, 1.0, kNumHistBins)
    , pathInteractions(0.0, 1.0, 0.0, 1.0, kNumHistBins)
    , pathDistAgent(0.0, 1.0, kNumHistBins)
    , gwyDistAgent(0.0, 1.0, kNumHistBins)
    {
    }
};

struct OverallResults
{
    VersionResults socialResults;
    VersionResults regularResults;

    MotionObs agentObsPath;
    MotionObs agentObsTrans;
};

void add_distance_results(const ResultsLog& log, VersionResults& results);
void add_interaction_results(const ResultsLog& log,
                             double maxDistance,
                             double ignoreConeAngle,
                             VersionResults& results);
void add_motion_interaction_results(const std::vector<interaction_t>& interactions,
                                    const hssh::LocalTopoMap& topoMap,
                                    VersionResults& results);
void add_social_forces_results(const std::vector<interaction_t>& interactions, VersionResults& results);
std::vector<passing_object_t> prune_passing_events(const std::vector<passing_object_t>& passes);
void accumulate_pass_results(const std::vector<passing_object_t>& passes, SituationResults& results);

StatResults populate_stat_results(StatsAcc& acc, const std::string& name);

void produce_results(OverallResults& results);
void produce_path_dist_results(OverallResults& results);
void produce_gwy_dist_results(OverallResults& results);
void produce_interaction_results(VersionResults& results);
void produce_situation_results(SituationResults& results, const std::string& name);
SituationResults* find_situation(const interaction_t& interaction, std::vector<SituationResults>& situations);
void draw_dist_result_histogram(utils::Histogram& regularHist, utils::Histogram& socialHist, const std::string& name);
void histogram_analysis(utils::Histogram& regularHist,
                        utils::Histogram& socialHist,
                        utils::Histogram& trainingHist,
                        const std::string& name);
void draw_interaction_histogram(utils::Histogram2D& hist, const std::string& title, double maxCbrange);
void passing_analysis(SituationResults& regularPass, SituationResults& socialPass, const std::string& name);
double kl_divergence(const utils::Histogram& from, const utils::Histogram& to);
std::ostream& operator<<(std::ostream& out, const StatResults& results);

int main(int argc, char** argv)
{
    std::vector<utils::command_line_argument_t> args = {
      {kDistArg, "Maximum distance for an interaction (meters)", true, "5"},
      {kConeArg, "Half angle of the ignore cone behind the robot (degrees)", true, "30"},
      {kLogsArg, "File containing the logs to process", false, ""},
      {kTrainArg, "Base name for training *obs and .situ file", false, ""}};

    utils::CommandLine cmdLine(argc, argv, args);

    if (!cmdLine.verify()) {
        cmdLine.printHelp();
        exit(EXIT_FAILURE);
    }

    double maxDistance = std::strtod(cmdLine.argumentValue(kDistArg).c_str(), 0);
    double ignoreConeAngle = wrap_to_pi(std::strtod(cmdLine.argumentValue(kConeArg).c_str(), 0) * M_PI / 180.0);
    ignoreConeAngle = angle_diff_abs(M_PI, ignoreConeAngle);

    auto logs = load_results_logs(cmdLine.argumentValue(kLogsArg));

    OverallResults allResults;
    allResults.socialResults.basename = cmdLine.argumentValue(kLogsArg);
    allResults.socialResults.version = version_to_public_name(MPEPCVersion::social);
    allResults.regularResults.basename = cmdLine.argumentValue(kLogsArg);
    allResults.regularResults.version = version_to_public_name(MPEPCVersion::regular);

    for (auto& log : logs) {
        VersionResults* results = nullptr;
        switch (log.version) {
        case MPEPCVersion::regular:
            results = &allResults.regularResults;
            break;
        case MPEPCVersion::social:
            results = &allResults.socialResults;
            break;
        case MPEPCVersion::unknown:
            std::cerr << "ERROR: Unknown MPEPC version for log.\n";
            break;
        }

        if (results) {
            add_distance_results(log, *results);
            add_interaction_results(log, maxDistance, ignoreConeAngle, *results);
        }
    }

    // Load training observations
    auto trainName = cmdLine.argumentValue(kTrainArg);

    // Check if there are .mobs for the log
    auto observations = load_motion_observations(trainName + ".pobs");

    for (auto& areaToObs : observations) {
        boost::push_back(allResults.agentObsPath, boost::as_array(areaToObs.second));
    }

    observations = load_motion_observations(trainName + ".tobs");

    for (auto& areaToObs : observations) {
        boost::push_back(allResults.agentObsTrans, boost::as_array(areaToObs.second));
    }

    std::cout << "Found " << allResults.agentObsPath.size() << " path observations and "
              << allResults.agentObsTrans.size() << " transition observations.\n";

    produce_results(allResults);

    return 0;
}

void add_distance_results(const ResultsLog& log, VersionResults& results)
{
    MPEPCLog logData(log.logName);

    const int64_t kChunkDurationUs = 5000000;

    // Process the log in chunks to ensure we don't run out of memory trying to load the big planning logs

    for (int64_t startTimeUs = 0; startTimeUs < logData.durationUs(); startTimeUs += kChunkDurationUs) {
        logData.loadTimeRange(startTimeUs, startTimeUs + (kChunkDurationUs * 2));

        for (auto& state : boost::make_iterator_range(logData.beginMotionState(startTimeUs),
                                                      logData.endMotionState(startTimeUs + kChunkDurationUs))) {
            auto agent = create_agent_for_robot(state, log.map);

            // If about to transition, process as a gateway distance
            if (is_about_to_transition(agent, log.map, 0.25)) {
                auto distance = normalized_position_gateway(agent, log.map);
                if (distance >= 0.0) {
                    results.gwyDist.addValue(distance);
                }
            }
            // If on a path, also a path distance here
            if (log.map.pathSegmentWithId(agent.areaId)) {
                auto distance = normalized_position_path(agent, log.map);
                if (distance >= 0.0) {
                    results.pathDist.addValue(distance);
                }
            }
        }
    }
}

void add_interaction_results(const ResultsLog& log, double maxDistance, double ignoreConeAngle, VersionResults& results)
{
    MPEPCLog logData(log.logName);
    // TODO: Make lateral bins a parameter?
    auto interactions = find_interactions(logData, log.map, 3, maxDistance, ignoreConeAngle);

    add_motion_interaction_results(interactions, log.map, results);
    add_social_forces_results(interactions, results);
}

void add_motion_interaction_results(const std::vector<interaction_t>& interactions,
                                    const hssh::LocalTopoMap& topoMap,
                                    VersionResults& results)
{
    // For every other agent in the environment that the robot interacts with, save the results of their behavior
    for (auto& interaction : interactions) {
        double robotDistance = normalized_position_path(interaction.robotAgent, topoMap);

        for (auto& agent : interaction.agents) {
            double vel = std::sqrt(std::pow(agent.state.xVel, 2.0) + std::pow(agent.state.yVel, 2.0));

            if ((vel < 0.25) || (vel > 2.0)) {
                continue;
            }

            // If about to transition, process as a gateway distance
            if (is_about_to_transition(agent, topoMap, 0.25)) {
                auto distance = normalized_position_gateway(agent, topoMap);
                if (distance >= 0.0) {
                    results.gwyDistAgent.addValue(distance);
                }
            }
            // If on a path, also a path distance
            if (topoMap.pathSegmentWithId(agent.areaId)) {
                auto distance = normalized_position_path(agent, topoMap);
                if (distance >= 0.0) {
                    results.pathDistAgent.addValue(distance);

                    results.pathInteractions.addValue(distance, robotDistance);
                }
            }
        }
    }
}

void add_social_forces_results(const std::vector<interaction_t>& interactions, VersionResults& results)
{
    social_forces_params_t params;   // Use defaults, which are taken from Ferrer's paper.
    auto forces = trajectory_social_forces(interactions, params);

    std::vector<passing_object_t> pathPasses;
    std::vector<passing_object_t> oncomingPasses;
    std::vector<passing_object_t> overtakingPasses;
    std::vector<passing_object_t> beingPassedPasses;

    for (std::size_t n = 0; n < forces.size(); ++n) {
        auto& f = forces[n];
        auto& interaction = interactions[n];

        SituationResults* situResults = find_situation(interaction, results.situations);

        if (f.isInteracting && interaction.pathSituation) {
            results.pathResults.forceAcc(f.force);
            results.pathResults.blameAcc(f.blame);

            if (situResults) {
                situResults->forceAcc(f.force);
                situResults->blameAcc(f.blame);
            }
        }

        // Find the closest passing object
        auto minIt = std::min_element(f.passingObj.begin(), f.passingObj.end(), [](const auto& lhs, const auto& rhs) {
            return lhs.passingDist < rhs.passingDist;
        });

        bool isPassing = minIt != f.passingObj.end();

        if (isPassing && interaction.pathSituation) {
            // Account for all passes
            boost::push_back(pathPasses, boost::as_array(f.passingObj));

            auto bins = interaction.pathSituation->situation();
            std::size_t towardCount = std::count_if(bins.begin(), bins.end(), [](PathSituation::BinState state) {
                return state == PathSituation::toward;
            });

            // If someone is coming toward us, then it is an oncoming pass
            if (towardCount > 0) {
                boost::push_back(oncomingPasses, boost::as_array(f.passingObj));

                results.oncomingResults.forceAcc(f.force);
                results.oncomingResults.blameAcc(f.blame);
            }
            // Otherwise, the relative speed determine who was doing the passing
            else if (minIt->passingSpeed > 0.0)   // Robot was overtaking the agent
            {
                boost::push_back(overtakingPasses, boost::as_array(f.passingObj));

                results.overtakingResults.forceAcc(f.force);
                results.overtakingResults.blameAcc(f.blame);
            }
            // Robot is being passed
            else {
                boost::push_back(beingPassedPasses, boost::as_array(f.passingObj));

                results.beingPassedResults.forceAcc(f.force);
                results.beingPassedResults.blameAcc(f.blame);
            }

            if (situResults) {
                situResults->distPassingAcc(minIt->passingDist);
                situResults->speedPassingAcc(minIt->passingSpeed);
            }
        }
    }

    pathPasses = prune_passing_events(pathPasses);
    accumulate_pass_results(pathPasses, results.pathResults);
    std::cout << "Found " << pathPasses.size() << " path passes.\n";

    oncomingPasses = prune_passing_events(oncomingPasses);
    accumulate_pass_results(oncomingPasses, results.oncomingResults);
    std::cout << "Found " << oncomingPasses.size() << " oncoming passes.\n";

    overtakingPasses = prune_passing_events(overtakingPasses);
    accumulate_pass_results(overtakingPasses, results.overtakingResults);
    std::cout << "Found " << overtakingPasses.size() << " overtaking passes.\n";

    beingPassedPasses = prune_passing_events(beingPassedPasses);
    accumulate_pass_results(beingPassedPasses, results.beingPassedResults);
    std::cout << "Found " << beingPassedPasses.size() << " being passed passes.\n";
}

std::vector<passing_object_t> prune_passing_events(const std::vector<passing_object_t>& passes)
{
    std::vector<passing_object_t> pruned;

    for (auto passIt = passes.begin(); passIt < passes.end();)   // increment happens internally
    {
        // Find the first object not near the
        auto passEnd = std::find_if(passIt, passes.end(), [passIt](const auto& pass) {
            return pass.id != passIt->id;
        });

        // Find the closest point during passing
        auto minIt = std::min_element(passIt, passEnd, [](const auto& lhs, const auto& rhs) {
            return lhs.passingDist < rhs.passingDist;
        });

        auto totalDist = std::accumulate(passIt, passEnd, 0.0, [](double total, const auto& pass) {
            return total + pass.passingDist;
        });

        // Create a summarized view of the pass, where we consider the mean of the passing distance, since boundary
        // estimation is noisy and imprecise
        passing_object_t obj = *minIt;
        obj.passingDist = totalDist / std::distance(passIt, passEnd);

        pruned.push_back(obj);
        passIt = passEnd;   // increment the search
    }

    return pruned;
}

void accumulate_pass_results(const std::vector<passing_object_t>& passes, SituationResults& results)
{
    for (auto& pass : passes) {
        results.distPassingAcc(pass.passingDist);
        results.speedPassingAcc(pass.passingSpeed);

        if (pass.passingSide == math::RectSide::left) {
            ++results.leftPassCount;
        } else if (pass.passingSide == math::RectSide::right) {
            ++results.rightPassCount;
        }
    }
}

StatResults populate_stat_results(StatsAcc& acc, const std::string& name)
{
    StatResults results;

    results.name = name;
    results.min = min(acc);
    results.max = max(acc);
    results.mean = mean(acc);
    results.stdDev = std::sqrt(variance(acc));
    results.median = median(acc);

    return results;
}

void produce_results(OverallResults& results)
{
    produce_path_dist_results(results);
    produce_gwy_dist_results(results);

    produce_interaction_results(results.socialResults);
    produce_interaction_results(results.regularResults);

    passing_analysis(results.regularResults.pathResults, results.socialResults.pathResults, "Analysis of All Passing");

    passing_analysis(results.regularResults.oncomingResults,
                     results.socialResults.oncomingResults,
                     "Analysis of Oncoming Passing");

    passing_analysis(results.regularResults.overtakingResults,
                     results.socialResults.overtakingResults,
                     "Analysis of Overtaking Passing");

    results.regularResults.pathInteractions.normalize();
    results.socialResults.pathInteractions.normalize();
    double maxInteraction =
      std::max(results.regularResults.pathInteractions.max(), results.socialResults.pathInteractions.max());

    draw_interaction_histogram(results.regularResults.pathInteractions,
                               results.regularResults.version + " Robot-Pedestrian Path Interaction Locations",
                               maxInteraction);
    draw_interaction_histogram(results.socialResults.pathInteractions,
                               results.socialResults.version + " Robot-Pedestrian Path Interaction Locations",
                               maxInteraction);
}

void produce_path_dist_results(OverallResults& results)
{
    utils::Histogram trainingData(0.0, 1.0, kNumHistBins);
    for (auto& obs : results.agentObsPath) {
        trainingData.addValue(obs.normDist);
    }

    draw_dist_result_histogram(results.regularResults.pathDist,
                               results.socialResults.pathDist,
                               "Distribution of Lateral Path Position");
    // Draw twice because it often fails the first time, since it is the first Gnuplot instance to open
    draw_dist_result_histogram(results.regularResults.pathDist,
                               results.socialResults.pathDist,
                               "Distribution of Lateral Path Position");
    histogram_analysis(results.regularResults.pathDist,
                       results.socialResults.pathDist,
                       trainingData,
                       "Analysis of Path Distance");

    if ((results.regularResults.pathDistAgent.numValues() > 0)
        && (results.socialResults.pathDistAgent.numValues() > 0)) {
        std::cout << "Found " << results.regularResults.pathDistAgent.numValues() << " regular path agents and "
                  << results.socialResults.pathDistAgent.numValues() << " social path agents.\n";

        draw_dist_result_histogram(results.regularResults.pathDistAgent,
                                   results.socialResults.pathDistAgent,
                                   "Distribution of Lateral Path Position for Pedestrians");
        histogram_analysis(results.regularResults.pathDistAgent,
                           results.socialResults.pathDistAgent,
                           trainingData,
                           "Analysis of Agent Path Distance");
    }
}

void produce_gwy_dist_results(OverallResults& results)
{
    utils::Histogram trainingData(0.0, 1.0, kNumHistBins);

    if (!results.agentObsTrans.empty()) {

        for (auto& obs : results.agentObsTrans) {
            trainingData.addValue(obs.normDist);
        }
    }

    draw_dist_result_histogram(results.regularResults.gwyDist,
                               results.socialResults.gwyDist,
                               "Distribution of Lateral Transition Position");
    histogram_analysis(results.regularResults.gwyDist,
                       results.socialResults.gwyDist,
                       trainingData,
                       "Analysis of Transition Distance");

    if ((results.regularResults.gwyDistAgent.numValues() > 0) && (results.socialResults.gwyDistAgent.numValues() > 0)) {
        std::cout << "Found " << results.regularResults.gwyDistAgent.numValues() << " regular gateway agents and "
                  << results.socialResults.gwyDistAgent.numValues() << " social gateway agents.\n";

        draw_dist_result_histogram(results.regularResults.gwyDistAgent,
                                   results.socialResults.gwyDistAgent,
                                   "Distribution of Lateral Transition Position for Pedestrians");
        histogram_analysis(results.regularResults.gwyDistAgent,
                           results.socialResults.gwyDistAgent,
                           trainingData,
                           "Analysis of Agent Transition Distance");
    }
}

void produce_interaction_results(VersionResults& results)
{
    produce_situation_results(results.pathResults, results.version + " path passing");
    produce_situation_results(results.oncomingResults, results.version + " oncoming passing");
    produce_situation_results(results.overtakingResults, results.version + " overtaking passing");
    produce_situation_results(results.beingPassedResults, results.version + " being passed passing");

    //     for(auto& situation : results.situations)
    //     {
    //         std::cout << results.version << " -- " << situation.situation << " :"
    //             << " count: " << count(situation.forceAcc) << "\n"
    //             << populate_stat_results(situation.forceAcc, "Force") << '\n'
    //             << populate_stat_results(situation.blameAcc, "Blame") << '\n';
    //
    //         if(count(situation.distPassingAcc) > 0)
    //         {
    //             std::cout << "Passing count: " << count(situation.distPassingAcc) << '\n'
    //                 << populate_stat_results(situation.distPassingAcc, "Passing Distance (m)") << '\n'
    //                 << populate_stat_results(situation.speedPassingAcc, "Passing Speed (m/s)") << '\n';
    //         }
    //     }
}

void produce_situation_results(SituationResults& results, const std::string& name)
{
    auto forceStats = populate_stat_results(results.forceAcc, "Force");
    auto blameStats = populate_stat_results(results.blameAcc, "Blame");
    auto distPassingStats = populate_stat_results(results.distPassingAcc, "Passing Distance (m)");
    auto speedPassingStats = populate_stat_results(results.speedPassingAcc, "Passing Speed (m/s)");

    std::cout << "\n==========   " << name << "   ==========\n\n" << forceStats << '\n' << blameStats << '\n';

    if (count(results.distPassingAcc) > 0) {
        std::cout << "Passing count:" << count(results.distPassingAcc) << '\n'
                  << distPassingStats << '\n'
                  << speedPassingStats << '\n';
    }

    int totalPass = results.leftPassCount + results.rightPassCount;
    if (totalPass > 0) {
        std::cout << "Left pass %:  " << (results.leftPassCount / static_cast<double>(totalPass)) << '\n'
                  << "Right pass %: " << (results.rightPassCount / static_cast<double>(totalPass)) << '\n';
    }
}

SituationResults* find_situation(const interaction_t& interaction, std::vector<SituationResults>& situations)
{
    if (!interaction.pathSituation && !interaction.placeSituation) {
        return nullptr;
    }

    // See if this situation matches something already
    for (auto& s : situations) {
        if (interaction.pathSituation && s.situation.isResponseForSituation(*interaction.pathSituation)) {
            return &s;
        } else if (interaction.placeSituation && s.situation.isResponseForSituation(*interaction.placeSituation)) {
            return &s;
        }
    }

    // Otherwise, add a new situation
    SituationResults r;
    std::vector<double> nullDist(2, 0.5);   // don't actually care about these
    if (interaction.pathSituation) {
        r.situation = TopoSituationResponse(*interaction.pathSituation, nullDist);
    } else {
        assert(interaction.placeSituation);
        r.situation = TopoSituationResponse(*interaction.placeSituation, nullDist);
    }

    situations.push_back(r);
    return &situations.back();
}

void draw_dist_result_histogram(utils::Histogram& regularHist, utils::Histogram& socialHist, const std::string& name)
{
    Gnuplot plot;
    plot << "set yrange [0:25]\n";
    plot << "set xrange [0:1]\n";
    plot << "set style fill solid border lc black\n";
    plot << "set title '" << name << "'\n";
    plot << "set xlabel 'Normalized Dist'\n";
    plot << "set ylabel '% of trajectory'\n";

    std::vector<uint32_t> rawColors = {
      (255U << 24) | (135U << 16) | (206U << 8) | (235U),
      (255U << 24) | (255U << 16) | (99U << 8) | (71U),
      (255U << 24) | (106U << 16) | (90U << 8) | (205U),
    };

    std::vector<std::string> colors;
    for (auto& c : rawColors) {
        std::ostringstream str;
        str << '#' << std::hex << ((c >> 16) & 0xFF) << ((c >> 8) & 0xFF) << (c & 0xFF);
        colors.push_back(str.str());
    }

    const std::string lineStyle(" lw 2 pi 1 ps 1");

    plot << "plot "
         << "'-' using 2:1 with lp lt 5 lc rgb \"" << colors[0] << "\"" << lineStyle << " title '"
         << version_to_public_name(MPEPCVersion::regular) << "'"
         << ", '-' using 2:1 with lp lt 7 dt 2 lc rgb \"" << colors[1] << "\"" << lineStyle << " title '"
         << version_to_public_name(MPEPCVersion::social) << "'";
    plot << std::endl;

    // Data is col 1 = value, col 2 = tic value
    std::vector<double> regularVals;
    std::vector<double> socialVals;
    std::vector<double> xtics;

    regularHist.normalize();
    for (auto& bin : regularHist) {
        regularVals.push_back(bin.count * 100);
        xtics.push_back(bin.minValue);
    }

    socialHist.normalize();
    xtics.clear();
    for (auto& bin : socialHist) {
        socialVals.push_back(bin.count * 100);
        xtics.push_back(bin.minValue);
    }

    plot.send1d(boost::make_tuple(regularVals, xtics));
    plot.send1d(boost::make_tuple(socialVals, xtics));

    sleep(1);
}

void histogram_analysis(utils::Histogram& regularHist,
                        utils::Histogram& socialHist,
                        utils::Histogram& trainingHist,
                        const std::string& name)
{
    regularHist.normalize();
    socialHist.normalize();
    trainingHist.normalize();

    auto regularDist = regularHist.toGaussian();
    auto socialDist = socialHist.toGaussian();

    math::t_test_sample_t sampleRegular = {regularDist.mean(),
                                           regularDist.variance(),
                                           static_cast<int>(regularHist.numValues())};

    math::t_test_sample_t sampleSocial = {socialDist.mean(),
                                          socialDist.variance(),
                                          static_cast<int>(socialHist.numValues())};

    auto tTest = math::independent_t_test(sampleSocial, sampleRegular, 0.05);

    std::cout << "\n\n=====    " << name << "   =====\n"
              << "KLD to regular: " << kl_divergence(trainingHist, regularHist) << '\n'
              << "KLD to social: " << kl_divergence(trainingHist, socialHist) << '\n'
              << "Regular dist (mean, var): " << regularDist.mean() << ',' << regularDist.variance() << '\n'
              << "Social dist (mean, var): " << socialDist.mean() << ',' << socialDist.variance() << '\n';

    // A value being greater than another here means it moves to the right of the other
    std::cout << "T-test results:\n"
              << "Num samples: Social: " << sampleSocial.numSamples << " Regular: " << sampleRegular.numSamples << '\n'
              << "Dists are different:      " << tTest.areDifferent << " p-value:" << tTest.pValueDifferent
              << " t-value: " << tTest.tValue << '\n'
              << "Social right of regular:  " << tTest.isGreater << " p-value: " << tTest.pValueGreater
              << " t-value: " << tTest.tValue << '\n'
              << "Social left of regular:   " << tTest.isLess << " p-value: " << tTest.pValueLess
              << " t-value: " << tTest.tValue << '\n';
    std::cout << "====================\n\n";
}

void draw_interaction_histogram(utils::Histogram2D& hist, const std::string& title, double maxCbrange)
{
    hist.plot(title, "Pedestrian Normalized Path Distance", "Robot Normalized Path Distance", maxCbrange);
}

void passing_analysis(SituationResults& regularPass, SituationResults& socialPass, const std::string& name)
{
    std::cout << "\n\n=====    " << name << "   =====\n";

    math::z_test_sample_t regular = {regularPass.leftPassCount, regularPass.leftPassCount + regularPass.rightPassCount};

    math::z_test_sample_t social = {socialPass.leftPassCount, socialPass.leftPassCount + socialPass.rightPassCount};

    auto zTest = math::binomial_proportion_z_test(social, regular, 0.05);

    // A value being greater than another here means it moves to the right of the other
    std::cout << "Z-test results:\n"
              << "Num samples: Social: " << social.numSamples << " Regular: " << regular.numSamples << '\n'
              << "Dists are different:       " << zTest.areDifferent << " p-value:" << zTest.pValueDifferent
              << " z-value: " << zTest.zValue << '\n'
              << "Social more left passes:  " << zTest.isGreater << " p-value: " << zTest.pValueGreater
              << " z-value: " << zTest.zValue << '\n'
              << "Social more right passes: " << zTest.isLess << " p-value: " << zTest.pValueLess
              << " z-value: " << zTest.zValue << '\n';
    std::cout << "====================\n\n";

    math::t_test_sample_t sampleRegular = {mean(regularPass.distPassingAcc),
                                           variance(regularPass.distPassingAcc),
                                           static_cast<int>(count(regularPass.distPassingAcc))};

    math::t_test_sample_t sampleSocial = {mean(socialPass.distPassingAcc),
                                          variance(socialPass.distPassingAcc),
                                          static_cast<int>(count(socialPass.distPassingAcc))};

    auto tTest = math::independent_t_test(sampleSocial, sampleRegular, 0.05);

    // A value being greater than another here means it moves to the right of the other
    std::cout << "T-test results for passing distance:\n"
              << "Num samples: Social: " << sampleSocial.numSamples << " Regular: " << sampleRegular.numSamples << '\n'
              << "Dists are different:     " << tTest.areDifferent << " p-value:" << tTest.pValueDifferent
              << " t-value: " << tTest.tValue << '\n'
              << "Social passes further:  " << tTest.isGreater << " p-value: " << tTest.pValueGreater
              << " t-value: " << tTest.tValue << '\n'
              << "Social passes closer:   " << tTest.isLess << " p-value: " << tTest.pValueLess
              << " t-value: " << tTest.tValue << '\n';
    std::cout << "====================\n\n";
}

double kl_divergence(const utils::Histogram& from, const utils::Histogram& to)
{
    assert(from.size() == to.size());

    const double kMinProb = 1e-4;

    double kld = 0.0;

    for (std::size_t n = 0; n < from.size(); ++n) {
        double fromProb = std::max(from.bin(n).count, kMinProb);
        double toProb = std::max(to.bin(n).count, kMinProb);

        kld += toProb * std::log(toProb / fromProb);
    }

    return kld;
}

std::ostream& operator<<(std::ostream& out, const StatResults& results)
{
    out << std::fixed << std::setprecision(3) << results.name << '\t' << results.min << '\t' << results.max << '\t'
        << results.mean << '\t' << results.stdDev << '\t' << results.median;

    return out;
}


#include "step_crawling.h"

#include <fstream>
#include <cstdlib>  // For srand() and rand()

#include <Eigen/SVD>
#include <RobotUtilities/utilities.h>
#include <RobotUtilities/TimerLinux.h>

#include <solvehfvc.h>

#define PI 3.14159265

using namespace RUT;
using std::cout;
using std::cerr;
using std::endl;
using std::string;
using Eigen::Vector2d;
using Eigen::Matrix2d;

bool StepCrawlingTaskServer::initStepCrawlingTaskServer() {
  ROS_INFO_STREAM("Step crawling server is starting");
  if (_ros_handle_p == nullptr) {
    ROS_ERROR_STREAM("[StepCrawlingTaskServer] You must call .init() before .initStepCrawlingTaskServer().");
    exit(1);
  }

  _ros_handle_p->param(string("/task/v_singular_value_threshold"),
      _v_singular_value_threshold, 0.1);
  _ros_handle_p->param(string("/task/f_singular_value_threshold"),
      _f_singular_value_threshold, 0.1);
  if (!_ros_handle_p->hasParam("/task/v_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/v_singular_value_threshold] not found");
  if (!_ros_handle_p->hasParam("/task/f_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/f_singular_value_threshold] not found");

  _ros_handle_p->param(std::string("/task/time_step"), _kTimeStepSec, 0.1);
  if (!_ros_handle_p->hasParam("/task/time_step"))
    ROS_WARN_STREAM("Parameter [/task/time_step] not found");
  _ros_handle_p->param(std::string("/task/number_of_time_steps"), _kNumOfTimeSteps, 30);
  if (!_ros_handle_p->hasParam("/task/number_of_time_steps"))
    ROS_WARN_STREAM("Parameter [/task/number_of_time_steps] not found");
  _ros_handle_p->param(std::string("/task/goal_veloicty_meter"), _kGoalVelocityM, 0.01);
  if (!_ros_handle_p->hasParam("/task/goal_veloicty_meter"))
    ROS_WARN_STREAM("Parameter [/task/goal_veloicty_meter] not found");
  _ros_handle_p->param(std::string("/task/engage_step_length"), _kEngageStepLength, 0.001);
  if (!_ros_handle_p->hasParam("/task/engage_step_length"))
    ROS_WARN_STREAM("Parameter [/task/engage_step_length] not found");
  _ros_handle_p->param(std::string("/task/min_normal_force"), _kNormalForceMin, 4.0);
  if (!_ros_handle_p->hasParam("/task/min_normal_force"))
    ROS_WARN_STREAM("Parameter [/task/min_normal_force] not found");

  _F_W(0) = 0;
  _F_W(1) = 1;
  _F_W(2) = 0;

  return true;
}

bool StepCrawlingTaskServer::hostServices() {
  // --------------------------------------------------------
  // Establish Services
  // --------------------------------------------------------
  ros::ServiceServer reset_service            = _ros_handle_p->advertiseService("reset", &StepCrawlingTaskServer::SrvReset, (RobotBridge*)this);
  ros::ServiceServer move_tool_service        = _ros_handle_p->advertiseService("move_tool", &StepCrawlingTaskServer::SrvMoveTool, (RobotBridge*)this);
  ros::ServiceServer move_until_touch_service = _ros_handle_p->advertiseService("move_until_touch", &StepCrawlingTaskServer::SrvMoveUntilTouch, (RobotBridge*)this);
  ros::ServiceServer get_pose_service         = _ros_handle_p->advertiseService("get_pose", &StepCrawlingTaskServer::SrvGetPose, (RobotBridge*)this);
  ros::ServiceServer execute_task_service     = _ros_handle_p->advertiseService("execute_task", &StepCrawlingTaskServer::SrvExecuteTask, this);

  cout << endl << "[StepCrawlingTaskServer] Service servers are listening.." << endl;
  ros::spin();

  ROS_INFO_STREAM(endl << "[StepCrawlingTaskServer] Service servers stopped." << endl);
  return true;
}

bool StepCrawlingTaskServer::SrvExecuteTask(std_srvs::Empty::Request  &req,
    std_srvs::Empty::Response &res) {
  _controller.reset();

  if (_controller._f_queue.size() < 50) {
    cout << "Run update() for " << _main_loop_rate << " frames:" << endl;
    // first, run update for 1s to populate the data deques
    ros::Rate pub_rate(_main_loop_rate);
    for (int i = 0; i < _main_loop_rate; ++i) {
      bool b_is_safe = _controller.update();
      // if(!b_is_safe) break;
      pub_rate.sleep();
    }
    cout << "Done." << endl;
  }

  Timer timer;
  std::srand(std::time(0));
  MatrixXd f_data, v_data;
  for (int fr = 0; fr < _kNumOfTimeSteps; ++fr) {
    timer.tic();
    /**
     * Estimate Natural Constraints
     */
    // get the weighted data
    f_data = MatrixXd::Zero(3, _controller._f_queue.size());
    v_data = MatrixXd::Zero(3, _controller._v_queue.size());
    for (int i = 0; i < _controller._f_queue.size(); ++i)
      f_data.col(i) = _controller._f_queue[i].head(3) * _controller._f_weights[i];
    for (int i = 0; i < _controller._v_queue.size(); ++i)
      v_data.col(i) = _controller._v_queue[i].head(3) * _controller._v_weights[i];

    // SVD on velocity data
    Eigen::JacobiSVD<MatrixXd> svd_v(v_data.transpose(), Eigen::ComputeThinV);
    VectorXd sigma_v = svd_v.singularValues();
    int DimV = 0;
    for (int i = 0; i < 3; ++i)
      if (sigma_v(i) > _v_singular_value_threshold) DimV ++;

    // get a basis for row space of velocity data
    MatrixXd rowspace_v = svd_v.matrixV().leftCols(DimV);

    // filter out force data that:
    //    1. has a small weight
    std::vector<int> f_id;
    for (int i = 0; i < f_data.cols(); ++i) {
      double length = f_data.col(i).norm();
      if (length > 1.5) { // weighted length in newton
        f_id.push_back(i);
      }
    }

    double pose_fb[7], pose_set[7];
    _robot.getPose(pose_fb);
    Matrix3d R_WT = quat2SO3(pose_fb[3], pose_fb[4], pose_fb[5], pose_fb[6]);

    int f_data_length = f_id.size();
    if (f_data_length < 5) {
      ROS_WARN_STREAM("[StepCrawlingTaskServer] lacks significant force data");
      // stabilize onto the surface.
      Vector3d F_T = R_WT.transpose() * _F_W.normalized() * _kNormalForceMin;
      double force_temp[6] = {- F_T(0), - F_T(1), - F_T(2), 0, 0, 0};
      _controller.ExecuteHFVC(6, 0, Matrix6d::Identity(), pose_fb, force_temp,
          HS_CONTINUOUS, _main_loop_rate, 10*_kTimeStepSec);
      continue;
    }
    int DimF = 1;

    MatrixXd f_data_filtered = MatrixXd::Zero(3, f_data_length);
    for (int i = 0; i < f_data_length; ++i)
      f_data_filtered.col(i) = f_data.col(f_id[i]);

    MatrixXd f_data_selected;
    // SVD on filtered force data, get the prime direction
    Eigen::JacobiSVD<MatrixXd> svd_f(f_data_filtered.transpose(), Eigen::ComputeThinV);
    Vector3d F_T = svd_f.matrixV().col(0).normalized();
    // check sign
    MatrixXd check_sum = F_T.transpose() * f_data_filtered * VectorXd::Ones(f_data_length);
    if (check_sum(0,0) < 0) F_T = -F_T;
    MatrixXd Nf = F_T.transpose();

    /**
     * Infer Goal
     */
    // Transform Nf to world frame
    _F_W = R_WT * F_T;
    cout << "F_W: " << _F_W.transpose().format(MatlabFmt) << endl;
    Vector3d goal_direction_W = _F_W.cross(-Vector3d::UnitX());
    cout << "goal_direction_W: " << goal_direction_W.transpose().format(MatlabFmt) << endl;

    MatrixXd G = MatrixXd::Zero(1, 3);
    VectorXd b_G = VectorXd::Zero(1);
    G = (R_WT.transpose() * goal_direction_W).transpose();
    b_G(0) = _kGoalVelocityM;

    /**
     * Do Hybrid Servoing (3D)
     */
    HFVC action;
    int kDimActualized      = 3;
    int kDimUnActualized    = 0;
    int kDimSlidingFriction = 0;
    int kNumSeeds           = 3;
    int kDimLambda          = 1;
    int kPrintLevel         = 2;

    VectorXd F = VectorXd::Zero(3);
    MatrixXd Aeq(0, 1+3); // dummy
    VectorXd beq(0); // dummy
    MatrixXd A = MatrixXd::Zero(1, 1 + 3);
    VectorXd b_A = VectorXd::Zero(1);
    A(0, 0) = -1;
    b_A(0) = -_kNormalForceMin;

    cout << "Nf: " << Nf.format(MatlabFmt) << endl;
    cout << "G: " << G.format(MatlabFmt) << endl;
    cout << "b_G: " << b_G.format(MatlabFmt) << endl;
    cout << "F: " << F.format(MatlabFmt) << endl;
    cout << "Aeq: " << Aeq.format(MatlabFmt) << endl;
    cout << "beq: " << beq.format(MatlabFmt) << endl;
    cout << "A: " << A.format(MatlabFmt) << endl;
    cout << "b_A: " << b_A.format(MatlabFmt) << endl;

    solvehfvc(Nf, G, b_G, F, Aeq, beq, A, b_A,
      kDimActualized, kDimUnActualized,
      kDimSlidingFriction, kDimLambda,
      kNumSeeds, kPrintLevel,
      &action);

    double computation_time_ms = timer.toc();

    /**
     * Execute the hybrid action (6D)
     */
    Vector6d v_Tr = Vector6d::Zero();
    for (int i = 0; i < action.n_av; ++i)  v_Tr(i+action.n_af) = action.w_av(i);

    // Tool frame velocity
    Matrix6d R_a;
    R_a << action.R_a, Matrix3d::Zero(), Matrix3d::Zero(), Matrix3d::Identity();
    Matrix6d R_a_inv = R_a.inverse();
    Vector6d v_T = R_a_inv*v_Tr;

    // World frame velocity, set pose
    _robot.getPose(pose_fb);
    Matrix4d SE3_WT_fb = posemm2SE3(pose_fb);
    Matrix6d Adj_WT = SE32Adj(SE3_WT_fb);
    Vector6d v_W = Adj_WT*v_T;
    Matrix4d SE3_WT_command;
    SE3_WT_command = SE3_WT_fb + wedge6(v_W)*SE3_WT_fb*_kTimeStepSec;
    SE32Posemm(SE3_WT_command, pose_set);

    // Tool frame force
    Vector6d force_Tr_set = Vector6d::Zero();
    for (int i = 0; i < action.n_af; ++i)  force_Tr_set[i] = action.eta_af(i);
    Vector6d force_T = R_a_inv*force_Tr_set;
    Matrix6d Adj_TW = SE32Adj(SE3Inv(SE3_WT_fb));

    Vector6d force_W = Adj_TW.transpose() * force_T;
    cout << "force_Tr_set: " << force_Tr_set.transpose().format(MatlabFmt) << endl;
    cout << "force_T: " << force_T.transpose().format(MatlabFmt) << endl;
    printf("V in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", v_W[0], v_W[1],
        v_W[2],v_W[3],v_W[4],v_W[5]);
    printf("F in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_W[0],
        force_W[1], force_W[2],force_W[3],force_W[4],force_W[5]);
    cout << "Computation Time: " << computation_time_ms << endl << endl;

    if (fr == _kNumOfTimeSteps-1) {
      // print to file
      std::ofstream fp;
      // f_queue
      fp.open(_task_data_file_path + "f_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_queue.size(); ++i) {
        stream_array_in(fp, _controller._f_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // v_queue
      fp.open(_task_data_file_path + "v_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_queue.size(); ++i) {
        stream_array_in(fp, _controller._v_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // f_weights
      fp.open(_task_data_file_path + "f_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_weights.size(); ++i)
        fp << _controller._f_weights[i] << endl;
      fp.close();

      // v_weights
      fp.open(_task_data_file_path + "v_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_weights.size(); ++i)
        fp << _controller._v_weights[i] << endl;
      fp.close();

      // f_data_filtered
      fp.open(_task_data_file_path + "f_data_filtered.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_filtered.transpose() << endl;
      fp.close();
      // f_data_selected
      fp.open(_task_data_file_path + "f_data_selected.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_selected.transpose() << endl;
      fp.close();
      // others
      fp.open(_task_data_file_path + "process.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << DimV << endl << DimF << endl;
      stream_array_in(fp, v_T.data(), 3);
      fp << endl;
      stream_array_in(fp, force_T.data(), 3);
      fp.close();
    }

    if (std::isnan(force_Tr_set[0])) {
      cout << "================== NaN =====================" << endl;
      cout << "Press Enter to continue.." << endl;
      getchar();
    }

    cout << "Current pose: " << pose_fb[0] << ", " << pose_fb[1] << ", " << pose_fb[2] << endl;
    cout << "Set pose: " << pose_set[0] << ", " << pose_set[1] << ", " << pose_set[2] << endl;
    cout << "Press Enter to begin motion: \n";
    // getchar();
    // cout << "motion begins:" << endl;
    _controller.ExecuteHFVC(action.n_af, action.n_av+3,
        R_a, pose_set, force_Tr_set.data(),
        HS_CONTINUOUS, _main_loop_rate, _kTimeStepSec);

  } // end for
  return true;
}