/****************************************************************************
 * Copyright (c) 2012-2019 by the DataTransferKit authors                   *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the DataTransferKit library. DataTransferKit is     *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include "DTK_BoostRangeAdapters.hpp"

#include <DTK_Box.hpp>
#include <DTK_DetailsAlgorithms.hpp>
#include <DTK_Point.hpp>

#include <Kokkos_Core.hpp>
#include <Teuchos_UnitTestHarness.hpp>

#include <boost/range/algorithm.hpp> // reverse_copy, replace_if, count, generate, count_if
#include <boost/range/algorithm_ext.hpp> // iota
#include <boost/range/numeric.hpp>       // accumulate

#include <random>
#include <sstream>

TEUCHOS_UNIT_TEST( BoostGeometryAdapters, Range )
{
    Kokkos::View<int[4], Kokkos::HostSpace> w( "w" );

    boost::iota( w, 0 );
    std::stringstream ss;
    boost::reverse_copy( w, std::ostream_iterator<int>( ss, " " ) );
    TEST_EQUALITY( ss.str(), "3 2 1 0 " );

    boost::replace_if( w, []( int i ) { return ( i > 1 ); }, -1 );
    TEST_COMPARE_ARRAYS( w, std::vector<int>( {0, 1, -1, -1} ) );

    TEST_EQUALITY( boost::count( w, -1 ), 2 );
    TEST_EQUALITY( boost::accumulate( w, 5 ), 4 );
}

TEUCHOS_UNIT_TEST( BoostGeometryAdapters, PointCloud )
{
    using DataTransferKit::Point;
    using DataTransferKit::Details::distance;
    double const seed = 3.14;
    std::default_random_engine generator( seed );
    std::uniform_real_distribution<double> distribution( -1., 1. );
    int const n = 10000;
    Kokkos::View<Point *, Kokkos::HostSpace> cloud( "cloud", n );
    boost::generate( cloud, [&distribution, &generator]() {
        Point p;
        p[0] = distribution( generator );
        p[1] = distribution( generator );
        p[2] = distribution( generator );
        return p;
    } );

    Point const origin = {0., 0., 0.};
    double const radius = 1.;
    // 4/3 pi 1^3 / 2^3
    double const pi = 6. *
                      static_cast<double>( boost::count_if(
                          cloud,
                          [origin, radius]( Point point ) {
                              return ( distance( point, origin ) <= radius );
                          } ) ) /
                      static_cast<double>( n );

    double const relative_tolerance = .05;
    TEST_FLOATING_EQUALITY( pi, 3.14, relative_tolerance );
}


/*!
 * @file
 * Calculating interstitials i.e. atoms that are away from ideal lattice in
 * each frame.
 *
 * default run cmd from project directory:
 * `mpirun -n 4 ./bin/interstitialcount "data/lammps/dump*.txt" 3.165 0 bcc
 * */
#include <array>
#include <iostream>
#include <stdexcept>
#include <tuple>
#include <vector>
#include <boost/mpi.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/reduces.hpp>
#include <ezl/algorithms/filters.hpp>
#include <ezl/algorithms/fromFile.hpp>

#include "AddOffset.cpp"

void icount(int argc, char* argv[]) {
  using namespace ezl;
  using std::vector;
  using std::tuple;

  if (argc < 5) {
    std::cerr << "Please give arguments as fileIn, latConst, origin, bcc/fcc, "
                 "output file tag(optional), check source for defaults.\n";
    return;
  }
  std::string ftag = "";
  if (argc > 5) ftag = std::string{argv[5]};

  const std::string outFile = "data/output/icount" + ftag + ".txt";
  const float toleranceRatio = 0.3;

  auto latConst = float(std::stof(argv[2]));
  auto o = std::stof(argv[3]);
  auto origin = std::array<float, 3> {{o, 0, 0}};
  auto obj = AddOffset(latConst, argv[4], origin);

  // frame, coords
  auto reader = fromFile<int, std::array<float, 3>>(argv[1])
                  .lammps().cols({6, 3, 4, 5});

  rise(reader)
      .map<2>(obj)
      .filter<4>(gt(latConst * toleranceRatio))
      .reduce<1>(count(), 0).inprocess().ordered()
      .reduceAll([](vector<tuple<int, int>> a) { // sorting the time-step counts
        sort(a.begin(), a.end());
        return a;
      })
        .dump(outFile)
      .run();
}

int main(int argc, char *argv[]) {
  boost::mpi::environment env(argc, argv, false);
  try {
    icount(argc, argv);
  } catch (const std::exception& ex) {
    std::cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    std::cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}


#include <frovedis.hpp>
#include <frovedis/ml/neighbors/knn_unsupervised.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;

int main(int argc, char** argv) {
  use_frovedis use(argc,argv);
  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("input,i", value<std::string>(), "path for input dense matrix")
    ("output,o", value<std::string>(), "path for output graphs")
    ("neighbors,k", value<int>(), "number of required neighbors (default: 2)")
    ("metric,m", value<std::string>(), "metric for distance calculation (default: euclidean)")
    ("algorithm,a", value<std::string>(), "algorithm for knn (default: brute)")
    ("radius,r", value<float>(), "radius for finding nearest neighbors (default: 1.0)")
    ("need_distance,d", value<int>(), "need distance as output (1/0) (default: 1)")
    ("chunk_size,s", value<float>(), "chunk size in MB for controlling runtime memory requirement (default: 1.0)");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  std::string input, output;
  int k = 2;
  float radius = 1.0;
  float chunk_size = 1.0;
  std::string metric = "euclidean";
  std::string algorithm = "brute";
  bool need_distance = true;

  if(argmap.count("help")){
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("input")){
    input = argmap["input"].as<std::string>();
  } else {
    std::cerr << "input path is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }
  auto data = make_rowmajor_matrix_load<double>(input);

  if(argmap.count("output")){
    output = argmap["output"].as<std::string>();
  } else {
    std::cerr << "output path is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("neighbors")){
    k = argmap["neighbors"].as<int>();
  }

  if(argmap.count("metric")){
    metric = argmap["metric"].as<std::string>();
  }

  if(argmap.count("algorithm")){
    algorithm = argmap["algorithm"].as<std::string>();
  }

  if(argmap.count("need_distance")){
    need_distance = argmap["need_distance"].as<int>() == 1;
  }

  if(argmap.count("chunk_size")){
    chunk_size = argmap["chunk_size"].as<float>();
  }

  if(argmap.count("radius")){
    radius = argmap["radius"].as<float>();
  }

  std::string mode = need_distance ? "distance" : "connectivity";

  time_spent calc_knn(INFO);
  calc_knn.lap_start();
  nearest_neighbors<double, rowmajor_matrix<double>> obj(k, radius, algorithm, metric, chunk_size);
  obj.fit(data);
  auto knn_graph = obj.kneighbors_graph(data, k, mode);
  calc_knn.lap_stop();
  calc_knn.show_lap("total knn + graph creation time: ");

  knn_graph.save(output + "/knn_graph");

  auto radius_graph = obj.radius_neighbors_graph(data, radius, mode);
  radius_graph.save(output + "/radius_graph");

  return 0;
}


#ifndef AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_
#define AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_
#include <Eigen/Core>
#include "ScopedState.hpp"
#include "StateSpace.hpp"

namespace aikido {
namespace statespace {

// Defined in detail/Rn-impl.hpp
template <class>
class RStateHandle;

/// Represents a N-dimensional real vector space with vector addition as the
/// group operation. \c N should be either non-negative or Eigen::Dynamic.
template <int N>
class R : public virtual StateSpace
{
public:
  /// Point in a \c R<N>.
  class State : public StateSpace::State
  {
  public:
    static constexpr int DimensionAtCompileTime = N;

  protected:
    State() = default;
    ~State() = default;

    friend class R<N>;
  };

  /// Dimension of the space
  static constexpr int DimensionAtCompileTime = N;

  using VectorNd = Eigen::Matrix<double, N, 1>;

  using StateHandle = RStateHandle<State>;
  using StateHandleConst = RStateHandle<const State>;

  using ScopedState = statespace::ScopedState<StateHandle>;
  using ScopedStateConst = statespace::ScopedState<StateHandleConst>;

  using StateSpace::compose;

  /// Constructs a \c N dimensional real vector space only when the dimension is
  /// can be known in compile time.
  ///
  /// If the dimension is known in compile time, it is recommended to use this
  /// constructor over R(int) because this constructor uses fixed size Eigen
  /// objects to represent the state data internally, which is generally
  /// faster than using dynamic size Eigen objects.
  ///
  /// \c N must be non-negative and not Eigen::Dynamic.
  ///
  /// \throw std::invalid_argument when N is Eigen::Dynamic.
  /// \sa R(int)
  R();

  /// Constructs a \c dimension dimensional real vector space.
  ///
  /// This constructor must be used only when N is Eigen::Dynamic (i.e., -1).
  /// Otherwise, throws an std::invalid_argument exception.
  ///
  /// \c N must be non-negative and not Eigen::Dynamic.
  ///
  /// \throw std::invalid_argument when \c N is not Eigen::Dynamic and
  /// \c dimension is not the same with \c N.
  /// \sa R()
  explicit R(int dimension);

  /// Helper function to create a \c ScopedState.
  ///
  /// \return new \c ScopedState
  ScopedState createState() const;

  /// Creates an identical clone of \c stateIn.
  ScopedState cloneState(const StateSpace::State* stateIn) const;

  /// Gets the real vector stored in a \c State.
  ///
  /// \param _state a \c State in this state space
  /// \return real vector represented by \c _state
  Eigen::Map<const VectorNd> getValue(const State* _state) const;

  /// Sets the real vector stored in a \c State.
  ///
  /// \param _state a \c State in this state space
  /// \param _value real vector to store in \c _state
  void setValue(State* _state, const VectorNd& _value) const;

  // Documentation inherited.
  std::size_t getStateSizeInBytes() const override;

  // Documentation inherited.
  StateSpace::State* allocateStateInBuffer(void* _buffer) const override;

  // Documentation inherited.
  void freeStateInBuffer(StateSpace::State* _state) const override;

  // Documentation inherited.
  void compose(
      const StateSpace::State* _state1,
      const StateSpace::State* _state2,
      StateSpace::State* _out) const override;

  // Documentation inherited
  void getIdentity(StateSpace::State* _out) const override;

  // Documentation inherited
  void getInverse(
      const StateSpace::State* _in, StateSpace::State* _out) const override;

  // Documentation inherited
  std::size_t getDimension() const override;

  // Documentation inherited
  void copyState(
      const StateSpace::State* _source,
      StateSpace::State* _destination) const override;

  /// Exponential mapping of Lie algebra element to a Lie group element. This
  /// is simply the identity transformation on a real vector space.
  ///
  /// \param _tangent element of the tangent space
  /// \param[out] _out corresponding element of the Lie group
  void expMap(
      const Eigen::VectorXd& _tangent, StateSpace::State* _out) const override;

  /// Log mapping of Lie group element to a Lie algebra element. This is simply
  /// an identity transformation on a real vector space.
  ///
  /// \param _in element of this Lie group
  /// \param[out] _tangent corresponding element of the tangent space
  void logMap(
      const StateSpace::State* _in, Eigen::VectorXd& _tangent) const override;

  /// Print the n-dimensional vector represented by the state
  /// Format: [x_1, x_2, ..., x_n]
  void print(const StateSpace::State* _state, std::ostream& _os) const override;

private:
  /// Gets the mutable value stored in a Rn::State. This is
  /// used internally to implement the public \c getValue member functions.
  ///
  /// \param _state element of this state space
  /// \return mutable reference to real vector stored in \c _state
  Eigen::Map<VectorNd> getMutableValue(State* _state) const;

  /// Dimension of the real vector space. Note that this value is only used for
  /// dynamic sized vector space (the dimension is changable).
  ///
  /// \warning Don't use this variable directly. Use getDimension() instead.
  /// \sa getDimension().
  int mDimension;
};

using R0 = R<0>;
using R1 = R<1>;
using R2 = R<2>;
using R3 = R<3>;
using R6 = R<6>;
using Rn = R<Eigen::Dynamic>;

} // namespace statespace
} // namespace aikido

#include "detail/Rn-impl.hpp"

#endif // ifndef AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_


#include <boost/program_options.hpp>

#include <cstdint>
#include <iostream>
#include <random>
#include <unordered_map>
#include <utility>

#include "recovery.h"
#include "stream.hpp"

int main(int argc, char *argv[])
{
    boost::program_options::options_description cli("Options");
    cli.add_options()
    ("help,h", "help")
    ("delta,d", boost::program_options::value<double>(), "delta parameter for k-sparse, defaults to 0.1")
    ("k-sparse,k", boost::program_options::value<uint16_t>(), "sparsity parameter for k-sparse, defaults to 10")
    ("l-wise,l", boost::program_options::value<uint8_t>(), "l-wise independent universal hash, defaults to 2")
    ("size,s", boost::program_options::value<uint16_t>(), "stream size of positive update, must be positive, defaults to 500")
    ("trial,t", boost::program_options::value<uint16_t>(), "number of trials, must be positive, defaults to 100");

    boost::program_options::variables_map options;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, cli), options);
    boost::program_options::notify(options);

    if (options.count("help"))
    {
        std::cout << cli << std::endl;
        return 0;
    }

    double d = 0.1;
    if (options.count("delta"))
    {
        d = options["delta"].as<double>();
        if (d <= 0 || d >= 1)
        {
            std::cerr << "-d requires a value in range (0, 1)" << std::endl;
            return 1;
        }
    }

    uint16_t k = 10;
    if (options.count("k-sparse"))
        k = options["k-sparse"].as<uint16_t>();

    uint8_t l = 2;
    if (options.count("l-wise"))
        l = options["l-wise"].as<uint8_t>();

    uint16_t s = 500;
    if (options.count("size"))
        s = options["size"].as<uint16_t>();

    uint16_t t = 100;
    if (options.count("trial"))
        t = options["trial"].as<uint16_t>();

    std::cout << "Turnstile Stream" << std::endl;
    for (uint8_t sparsity = 0; sparsity <= 2 * k; ++sparsity)
    {
        uint16_t ns = s - sparsity;

        std::vector<recovery::sparse_k> sparses;
        sparses.reserve(t);
        for (uint16_t i = 0; i < t; ++i)
            sparses.emplace_back(k, d, l);

        std::unordered_map<uint16_t, int64_t> record;
        stream::int_sparse_stream<stream::stream_type::turnstile> ts(s, ns);
        while (true)
        {
            std::pair<bool, std::pair<uint16_t, int8_t>> r = ts;
            if (!r.first)
                break;
            auto const & [item, update] = r.second;
            for (auto & sparse : sparses)
                sparse(item, update);
            record[item] += update;
            if (!record[item])
                record.erase(item);
        }

        uint16_t correct = 0;
        for (auto const & sparse: sparses)
        {
            std::unordered_map<uint16_t, int64_t> output = sparse;
            if (sparsity == 0 || sparsity > k)
                correct += output.size() == 0;
            else
                correct += output == record;
        }
        std::cout << "    Correct rate (" << s << " : " << ns << "): " << correct << "/" << t << std::endl;
    }

    std::cout << "General Stream" << std::endl;
    for (uint8_t sparsity = 0; sparsity <= 2 * k; ++sparsity)
    {
        uint16_t ns = s - sparsity;

        std::vector<recovery::sparse_k> sparses;
        sparses.reserve(t);
        for (uint16_t i = 0; i < t; ++i)
            sparses.emplace_back(k, d, l);

        std::unordered_map<uint16_t, int64_t> record;
        stream::int_sparse_stream<stream::stream_type::general> gs(s, ns);
        while (true)
        {
            std::pair<bool, std::pair<uint16_t, int8_t>> r = gs;
            if (!r.first)
                break;
            auto const & [item, update] = r.second;
            for (auto & sparse : sparses)
                sparse(item, update);
            record[item] += update;
            if (!record[item])
                record.erase(item);
        }

        uint16_t correct = 0;
        for (auto const & sparse: sparses)
        {
            std::unordered_map<uint16_t, int64_t> output = sparse;
            if (sparsity == 0 || sparsity > k)
                correct += output.size() == 0;
            else
                correct += output == record;
        }
        std::cout << "    Correct rate (" << s << " : " << ns << "): " << correct << "/" << t << std::endl;
    }

    return 0;
}


//
//! Copyright © 2018
//! Brandon Kohn
//
//  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)
//
#pragma once

#include <geometrix/numeric/detail/friend_ordered_field_operators.hpp>
#include <geometrix/numeric/constants.hpp>
#include <stk/container/icl/interval.hpp>
#include <boost/numeric/interval.hpp>
#include <boost/type_traits.hpp>
#include <limits>

namespace geometrix {

    template <typename Domain>
    class interval;

    #define GEOMETRIX_IMPLEMENT_ARITHMETIC_OP(Op)\
        template <typename U, typename std::enable_if<!std::is_same<interval<T>, U>::value, int>::type = 0>\
        interval<T>& operator Op(const U& rhs)\
        {\
            self()->internal_rep() Op rhs;\
            return *self();\
        }\
        template <typename U>\
        interval<T>& operator Op(const interval<U>& rhs)\
        {\
            self()->internal_rep() Op rhs.internal_rep();\
            return *self();\
        }\
    /***/

    namespace detail{

        template <typename Domain>
        struct interval_storage
        {
            using domain_type = Domain;

            interval_storage()
                : m_lower{ std::numeric_limits<domain_type>::has_infinity ? std::numeric_limits<domain_type>::infinity() : constants::one<domain_type>() }
                , m_upper{ std::numeric_limits<domain_type>::has_infinity ? -std::numeric_limits<domain_type>::infinity() : constants::zero<domain_type>() }
            {}

            interval_storage(const domain_type& v)
                : m_lower{ v }
                , m_upper{ v }
            {}

            interval_storage(const domain_type& l, const domain_type& u)
                : m_lower{ l }
                , m_upper{ u }
            {}

            bool is_empty() const
            {
                return !(m_lower <= m_upper);
            }

            const domain_type& lower() const
            {
                return m_lower;
            }

            const domain_type& upper() const
            {
                return m_upper;
            }

            void set_lower(const domain_type& v)
            {
                m_lower = v;
            }

            void set_upper(const domain_type& v)
            {
                m_upper = v;
            }

            void set(const domain_type& l, const domain_type& u)
            {
                m_lower = l;
                m_upper = u;
            }

            interval_storage& operator +=(const domain_type& v)
            {
                m_lower += v;
                m_upper += v;
                return *this;
            }

            interval_storage& operator +=(const interval_storage& v)
            {
                m_lower += v.m_lower;
                m_upper += v.m_upper;
                return *this;
            }

            interval_storage& operator -=(const domain_type& v)
            {
                m_lower -= v;
                m_upper -= v;
                return *this;
            }

            interval_storage& operator -=(const interval_storage& v)
            {
                m_lower -= v.m_lower;
                m_upper -= v.m_upper;
                return *this;
            }

            interval_storage& operator *=(const domain_type& v)
            {
                m_lower *= v;
                m_upper *= v;
                return *this;
            }

            interval_storage& operator /=(const domain_type& v)
            {
                m_lower /= v;
                m_upper /= v;
                return *this;
            }

        private:

            domain_type m_lower;
            domain_type m_upper;
        };

        template <typename Derived>
        struct crtp_base
        {
            Derived* self() { return static_cast<Derived*>(this); }
            const Derived* self() const { return static_cast<const Derived*>(this); }
        };

        template<typename Derived, typename Base = crtp_base<Derived>, typename EnableIf=void>
        struct interval_plus : Base{};

        template<typename T, typename Base>
        struct interval_plus<interval<T>, Base, typename std::enable_if<boost::has_plus<typename std::decay<T>::type>::value>::type>
        : Base
        {
            using crtp_base<interval<T>>::self;
            GEOMETRIX_IMPLEMENT_ARITHMETIC_OP(+=);
            GEOMETRIX_FRIEND_BINARY_OPERATOR_COMMUTATIVE(interval<T>, T, +);
            GEOMETRIX_FRIEND_BINARY_OPERATOR_COMMUTATIVE_SELF(interval<T>, +);
        };

        template<typename Derived, typename Base = crtp_base<Derived>, typename EnableIf=void>
        struct interval_minus : Base{};

        template<typename T, typename Base>
        struct interval_minus<interval<T>, Base, typename std::enable_if<boost::has_minus<typename std::decay<T>::type>::value>::type>
        : Base
        {
            using crtp_base<interval<T>>::self;
            GEOMETRIX_IMPLEMENT_ARITHMETIC_OP(-=);
            GEOMETRIX_FRIEND_BINARY_OPERATOR_NON_COMMUTATIVE(interval<T>, T, -);
            GEOMETRIX_FRIEND_BINARY_OPERATOR_NON_COMMUTATIVE_SELF(interval<T>, -);
        };

        template<typename Derived, typename Base = crtp_base<Derived>, typename EnableIf=void>
        struct interval_multiplies : Base{};

        template<typename T, typename Base>
        struct interval_multiplies<interval<T>, Base, typename std::enable_if<boost::has_multiplies<typename std::decay<T>::type>::value>::type>
        : Base
        {
            using crtp_base<interval<T>>::self;
            GEOMETRIX_IMPLEMENT_ARITHMETIC_OP(*=);
            GEOMETRIX_FRIEND_BINARY_OPERATOR_COMMUTATIVE(interval<T>, T, *);
            //GEOMETRIX_FRIEND_BINARY_OPERATOR_COMMUTATIVE_SELF(interval<T>, *);
        };

        template<typename Derived, typename Base = crtp_base<Derived>, typename EnableIf=void>
        struct interval_divides : Base{};

        template<typename T, typename Base>
        struct interval_divides<interval<T>, Base, typename std::enable_if<boost::has_divides<typename std::decay<T>::type>::value>::type>
        : Base
        {
            using crtp_base<interval<T>>::self;
            GEOMETRIX_FRIEND_BINARY_OPERATOR_NON_COMMUTATIVE(interval<T>, T, /);
            //GEOMETRIX_FRIEND_BINARY_OPERATOR_NON_COMMUTATIVE_SELF(interval<T>, /);
            GEOMETRIX_IMPLEMENT_ARITHMETIC_OP(/=);
        };
    }//! namespace detail;

    //! By default the storage type uses boost::numeric_interval. This works fine for arithmetic types and boost units.
    //! This should be specialized for things like vectors used to represent angle ranges.
    template <typename T, typename EnableIf=void>
    struct interval_storage_type
    {
        using type = detail::interval_storage<T>;
    };

    template <typename Domain>
    class interval
    : public detail::interval_plus<interval<Domain>
    , detail::interval_minus<interval<Domain>
    , detail::interval_divides<interval<Domain>
    , detail::interval_multiplies<interval<Domain>
      >>>>
    {
    public:

        using domain_type = Domain;
        using interval_bounds = stk::icl::interval_bounds;
        using internal_rep_type = typename interval_storage_type<domain_type>::type;
        using bounded_domain_type = typename stk::icl::bounded_value<Domain>::type;
        BOOST_CONCEPT_ASSERT((boost::DefaultConstructibleConcept<domain_type>));

        static interval open(const domain_type& lo, const domain_type& up){ return interval(lo, up, interval_bounds::open()); }
        static interval closed(const domain_type& lo, const domain_type& up){ return interval(lo, up, interval_bounds::closed()); }
        static interval closed(const domain_type& v){ return interval(v, v, interval_bounds::closed()); }
        static interval right_open(const domain_type& lo, const domain_type& up){ return interval(lo, up, interval_bounds::right_open()); }
        static interval left_open(const domain_type& lo, const domain_type& up){ return interval(lo, up, interval_bounds::left_open()); }

        interval()
            : m_interval{}
            , m_bounds(interval_bounds::right_open())
        {}

        explicit interval(const domain_type& v)
            : m_interval(v)
            , m_bounds(interval_bounds::closed())
        {}

        template <typename T>
        interval(const boost::numeric::interval<T>& v)
            : m_interval(v)
            , m_bounds(interval_bounds::closed())
        {}

        interval(const domain_type& low, const domain_type& up, stk::icl::interval_bounds bounds = stk::icl::interval_bounds::right_open())
            : m_interval(low, up)
            , m_bounds(bounds)
        {}

        domain_type     lower() const { return m_interval.lower(); }
        domain_type     upper() const { return m_interval.upper(); }
        interval_bounds bounds() const { return m_bounds; }

        bool            is_empty() const
        {
            return m_interval.is_empty();
        }

        interval&       expand(const domain_type& v)
        {
            auto was_empty = is_empty();
            if (!was_empty && v < lower())
                m_interval.set_lower(v);
            else if(!was_empty && v > upper())
                m_interval.set_upper(v);
            else if (was_empty)
                m_interval.set(v, v);
            return *this;
        }

        template <typename NumberComparisonPolicy>
        interval&       expand(const domain_type& v, const NumberComparisonPolicy& cmp)
        {
            if (cmp.less_than(v, lower()))
                m_interval.set(v, upper());
            else if(cmp.greater_than(v, upper()))
                m_interval.set(lower(), v);
            return *this;
        }

        template <typename U, typename NumberComparisonPolicy>
        bool intersects(const interval<U>& rhs, const NumberComparisonPolicy& cmp) const
        {
            return cmp.less_than(rhs.lower(), upper()) && cmp.less_than(lower(), rhs.upper());
        }

        template <typename U>
        bool intersects(const interval<U>& rhs) const
        {
            return (rhs.lower() < upper()) && (lower() < rhs.upper());
        }

        internal_rep_type& internal_rep()
        {
            return m_interval;
        }

    private:
        internal_rep_type         m_interval;
        stk::icl::interval_bounds m_bounds;
    };
}//! namespace geometrix;

namespace stk { namespace icl {

    template<typename DomainT>
    struct interval_traits<geometrix::interval<DomainT>>
    {
        using type = interval_traits;
        using domain_type = DomainT;
        using domain_compare = std::less<domain_type>;
        using interval_type = geometrix::interval<DomainT>;

        static interval_type construct(const domain_type& lo, const domain_type& up)
        {
            return interval_type(lo, up);
        }

        static domain_type lower(const interval_type& inter_val){ return inter_val.lower(); };
        static domain_type upper(const interval_type& inter_val){ return inter_val.upper(); };
    };

    template<typename DomainT>
    struct dynamic_interval_traits<geometrix::interval<DomainT> >
    {
        using type = dynamic_interval_traits;
        using interval_type = geometrix::interval<DomainT>;
        using domain_type = DomainT;
        using domain_compare = std::less<domain_type>;

        static interval_type construct(const domain_type& lo, const domain_type& up, interval_bounds bounds)
        {
            return interval_type(lo, up, bounds);
        }

        static interval_type construct_bounded(const bounded_value<DomainT>& lo, const bounded_value<DomainT>& up)
        {
            return interval_type(lo.value(), up.value(), lo.bound().left() | up.bound().right());
        }
    };

    template <typename DomainT>
    struct interval_bound_type<geometrix::interval<DomainT>>
    {
        using type = interval_bound_type<geometrix::interval<DomainT>>;
        BOOST_STATIC_CONSTANT(bound_type, value = interval_bounds::dynamic);
    };

    template <typename DomainT>
    struct is_interval<geometrix::interval<DomainT>>
    {
        using type = is_interval<geometrix::interval<DomainT>>;
        BOOST_STATIC_CONSTANT(bool, value = true);
    };

    template <typename Domain>
    struct is_continuous_interval<geometrix::interval<Domain>>
    {
        using type = is_continuous_interval<geometrix::interval<Domain>>;
        BOOST_STATIC_CONSTANT(bool, value = is_continuous<Domain>::value);
    };

    template <typename Domain>
    struct is_discrete_interval<geometrix::interval<Domain>>
    {
        using type = is_discrete_interval<geometrix::interval<Domain>>;
        BOOST_STATIC_CONSTANT(bool, value = is_discrete<Domain>::value);
    };

    template <typename DomainT>
    struct type_to_string<geometrix::interval<DomainT>>
    {
        static std::string apply() { return "cI<"+ type_to_string<DomainT>::apply() +">"; }
    };

    template<typename DomainT>
    struct value_size<geometrix::interval<DomainT> >
    {
        static std::size_t apply(const geometrix::interval<DomainT>&) { return 2; }
    };

}}//! namespace stk::icl;

#undef GEOMETRIX_IMPLEMENT_ARITHMETIC_OP


/*!
 * @file
 * Demo handling some real data.
 *
 * The data used is from:
 * http://ikdd.acm.org/Site/CoDS2016/datachallenge.html
 *
 * To see an example on logistic regression see example `logreg`.
 * */
#include <array>
#include <iostream>
#include <stdexcept>

#include <boost/mpi.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/filters.hpp>
#include <ezl/algorithms/maps.hpp>
#include <ezl/algorithms/fromFile.hpp>
#include <ezl/algorithms/reduces.hpp>
#include <ezl/algorithms/reduceAlls.hpp>

using namespace std;

void cods() {
  auto feat1 =
      ezl::fromFile<float, array<float, 4>, char>(
          "data/datachallenge_cods2016/train.csv")
          .cols({"Salary", "English", "Logical", "Quant", "Domain", "Gender"})
          .colSeparator("\t");

  auto source = ezl::rise(feat1)
                    .reduce(ezl::count(), 0).dump("", "Total rows:").oneUp()
                  .filter<2>(ezl::gtAr<4>(0))
                  .build();

  ezl::flow(source)
    .map<3>([](char gender) { return float(gender == 'M' || gender == 'm'); })
      .colsTransform()
    .map<2, 3>(ezl::mergeAr()).colsTransform()
    .reduceAll(ezl::corr<1>()).dump("", "corr. salary, e,l,q, domain, gender")
    .run();
}

int main(int argc, char *argv[]) {
  boost::mpi::environment env(argc, argv, false);
  try {
    cods();
  } catch (const exception& ex) {
    std::cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    std::cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, Clearpath Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#include <fuse_constraints/normal_prior_pose_2d.h>
#include <fuse_constraints/normal_prior_pose_2d_cost_functor.h>

#include <benchmark/benchmark.h>

#include <ceres/autodiff_cost_function.h>
#include <Eigen/Dense>

#include <vector>

class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture
{
public:
  NormalPriorPose2DBenchmarkFixture()
    : jacobians(num_parameter_blocks)
    , J(num_parameter_blocks)
  {
    for (size_t i = 0; i < num_parameter_blocks; ++i)
    {
      J[i].resize(num_residuals, block_sizes[i]);
      jacobians[i] = J[i].data();
    }
  }

  // Mean and sqrt information matrix
  static const fuse_core::Vector3d mean;
  static const fuse_core::Matrix3d sqrt_information;

  // Parameters
  static const double* parameters[];

  // Residuals
  fuse_core::Vector3d residuals;

  static const std::vector<int32_t>& block_sizes;
  static const size_t num_parameter_blocks;

  static const size_t num_residuals;

  // Jacobians
  std::vector<double*> jacobians;

private:
  // Cost function covariance
  static const double covariance_diagonal[];

  static const fuse_core::Matrix3d covariance;

  // Parameter blocks
  static const double position[];
  static const double orientation[];

  // Jacobian matrices
  std::vector<fuse_core::MatrixXd> J;
};

// Cost function covariance
const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };

const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::covariance =
    fuse_core::Vector3d(covariance_diagonal).asDiagonal();

// Parameter blocks
const double NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 };
const double NormalPriorPose2DBenchmarkFixture::orientation[] = {0.0};

// Mean and sqrt information matrix
const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{ 1.0, 2.0, 3.0 };
const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU());

// Parameters
const double* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation };

const std::vector<int32_t>& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 };
const size_t NormalPriorPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size();

const size_t NormalPriorPose2DBenchmarkFixture::num_residuals = 3;

BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)(benchmark::State& state)
{
  // Create analytic cost function
  const fuse_constraints::NormalPriorPose2D cost_function{ sqrt_information.topRows(state.range(0)), mean };

  for (auto _ : state)
  {
    cost_function.Evaluate(parameters, residuals.data(), jacobians.data());
  }
}

BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)->DenseRange(1, 3);

BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)(benchmark::State& state)
{
  // Create cost function using automatic differentiation on the cost functor
  const auto partial_sqrt_information = sqrt_information.topRows(state.range(0));
  const ceres::AutoDiffCostFunction<fuse_constraints::NormalPriorPose2DCostFunctor, ceres::DYNAMIC, 2, 1>
      cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, mean),
                             partial_sqrt_information.rows());

  for (auto _ : state)
  {
    cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data());
  }
}

BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)->DenseRange(1, 3);

BENCHMARK_MAIN();


#include "refine.h"

#include "salt/base/eval.h"
#include "salt/base/mst.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/index/rtree.hpp>

namespace salt {

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

using BPoint = bg::model::point<DTYPE, 2, bg::cs::cartesian>;
using BSegment = bg::model::segment<BPoint>;
using BBox = bg::model::box<BPoint>;
using BPolygon = bg::model::polygon<BPoint>;
using RNode = pair<BBox, shared_ptr<TreeNode>>;  // R-Tree node
struct RNodeComp {
    bool operator()(const RNode& l, const RNode& r) const {
        return bg::equals(l.first, r.first) && l.second == r.second;
    }
};

void Refine::Substitute(Tree& tree, double eps, bool useRTree) {
    bgi::rtree<RNode, bgi::rstar<8>, bgi::indexable<RNode>, RNodeComp> rtree;
    if (useRTree) {
        tree.PostOrder([&](const shared_ptr<TreeNode>& n) {
            if (n->parent) {
                BBox s;
                bg::envelope(BSegment(BPoint(n->loc.x, n->loc.y), BPoint(n->parent->loc.x, n->parent->loc.y)), s);
                rtree.insert({s, n});
            }
        });
    }
    auto Disconnect = [&](const shared_ptr<TreeNode>& n) {
        if (useRTree) {
            BBox s;
            bg::envelope(BSegment(BPoint(n->loc.x, n->loc.y), BPoint(n->parent->loc.x, n->parent->loc.y)), s);
            rtree.remove({s, n});
        }
        TreeNode::ResetParent(n);
    };
    auto Connect = [&](const shared_ptr<TreeNode>& n, const shared_ptr<TreeNode>& parent) {
        TreeNode::SetParent(n, parent);
        if (useRTree) {
            BBox s;
            bg::envelope(BSegment(BPoint(n->loc.x, n->loc.y), BPoint(parent->loc.x, parent->loc.y)), s);
            rtree.insert({s, n});
        }
    };
    while (true) {
        // Get nearest neighbors
        int num = tree.UpdateId();
        vector<shared_ptr<TreeNode>> nodes = tree.ObtainNodes(),
                                     orderedNodes(nodes.size());  // note: all pins should be covered
        vector<Point> points(nodes.size());
        for (int i = 0; i < nodes.size(); ++i) {
            orderedNodes[nodes[i]->id] = nodes[i];
            points[nodes[i]->id] = nodes[i]->loc;  // TODO: move within bracket
        }
        nodes = orderedNodes;
        vector<vector<int>> nearestNeighbors;
        if (!useRTree) {
            MstBuilder mstB;
            mstB.GetAllNearestNeighbors(points, nearestNeighbors);
        } else {
            nearestNeighbors.resize(nodes.size());
            for (auto n : nodes) {
                if (n->parent) {
                    Point c = n->loc;  // center
                    DTYPE radius = n->WireToParent();
                    // diamond is too slow...
                    // BPolygon diamond;
                    // diamond.outer().emplace_back(c.x - radius, c.y);
                    // diamond.outer().emplace_back(c.x, c.y + radius);
                    // diamond.outer().emplace_back(c.x + radius, c.y);
                    // diamond.outer().emplace_back(c.x, c.y - radius);
                    // diamond.outer().emplace_back(c.x - radius, c.y);
                    BBox queryBox{{c.x - radius, c.y - radius}, {c.x + radius, c.y + radius}};
                    vector<RNode> cands;
                    rtree.query(bgi::intersects(queryBox), back_inserter(cands));  // TODO: change back_inserter
                    for (const auto& cand : cands) {
                        nearestNeighbors[n->id].push_back(cand.second->id);
                    }
                }
            }
        }

        // Prune descendants in nearest neighbors
        vector<int> preOrderIdxes(nodes.size(), -1);
        int globalPreOrderIdx = 0;
        function<void(const shared_ptr<TreeNode>&)> removeDescendants = [&](const shared_ptr<TreeNode>& node) {
            preOrderIdxes[node->id] = globalPreOrderIdx++;
            for (auto child : node->children) {
                removeDescendants(child);
            }
            for (auto& neighIdx : nearestNeighbors[node->id]) {
                int neighPreOrderIdx = preOrderIdxes[neighIdx];
                if (neighPreOrderIdx != -1 && neighPreOrderIdx >= preOrderIdxes[node->id]) {
                    neighIdx = -1;  // -1 stands for "descendant"
                }
            }
        };
        removeDescendants(tree.source);

        // Init path lengths and subtree slacks
        vector<DTYPE> pathLengths(nodes.size());
        vector<DTYPE> slacks(nodes.size());
        auto UpdatePathLengths = [&](const shared_ptr<TreeNode>& node) {
            if (node->parent) {
                pathLengths[node->id] = pathLengths[node->parent->id] + node->WireToParent();
            } else {
                pathLengths[node->id] = 0;
            }
        };
        auto UpdateSlacks = [&](const shared_ptr<TreeNode>& node) {
            if (node->children.empty()) {
                slacks[node->id] =
                    Dist(node->loc, tree.source->loc) * (1 + eps) - pathLengths[node->id];  // floor here...
            } else {
                DTYPE minSlack = Dist(node->loc, tree.source->loc) * (1 + eps) - pathLengths[node->id];
                for (auto child : node->children) {
                    minSlack = min(minSlack, slacks[child->id]);
                }
                slacks[node->id] = minSlack;
            }
        };
        tree.PreOrder(UpdatePathLengths);
        tree.PostOrder(UpdateSlacks);

        // Find legal candidate moves
        using MoveT = tuple<DTYPE, shared_ptr<TreeNode>, shared_ptr<TreeNode>>;
        vector<MoveT> candidateMoves;  // <wireLengthDelta, node, newParent>
        auto GetNearestPoint = [](const shared_ptr<TreeNode>& target, const shared_ptr<TreeNode>& neigh) {
            Box box(neigh->loc, neigh->parent->loc);
            box.Legalize();
            return box.GetNearestPointTo(target->loc);
        };
        for (auto node : nodes) {
            if (!(node->parent)) {
                continue;
            }
            DTYPE bestWireLengthDelta = 0;  // the negative, the better
            shared_ptr<TreeNode> bestNewParent;
            for (int neighIdx : nearestNeighbors[node->id]) {
                if (neighIdx == -1 || !nodes[neighIdx]->parent) continue;
                auto neigh = nodes[neighIdx];
                auto neighParent = neigh->parent;
                auto steinerPt = GetNearestPoint(node, neigh);
                DTYPE wireLengthDelta = Dist(node->loc, steinerPt) - node->WireToParent();
                if (wireLengthDelta < bestWireLengthDelta) {  // has wire length improvement
                    DTYPE pathLengthDelta =
                        pathLengths[neighParent->id] + Dist(node->loc, neighParent->loc) - pathLengths[node->id];
                    if (pathLengthDelta <= slacks[node->id]) {  // make path length under control
                        bestWireLengthDelta = wireLengthDelta;
                        bestNewParent = neigh;
                    }
                }
            }
            if (bestNewParent) {
                candidateMoves.emplace_back(bestWireLengthDelta, node, bestNewParent);
            }
        }
        if (candidateMoves.empty()) {
            break;
        }

        // Try candidate moves in the order of descending wire length savings
        // Note that earlier moves may influence the legality of later one
        sort(candidateMoves.begin(), candidateMoves.end(), [](const MoveT& lhs, const MoveT& rhs){
            return get<0>(lhs) < get<0>(rhs);
        });
        for (const auto& move : candidateMoves) {
            auto node = get<1>(move), neigh = get<2>(move);
            auto neighParent = neigh->parent;
            // check due to earlier moves
            if (TreeNode::IsAncestor(node, neighParent)) continue;
            DTYPE pathLengthDelta =
                pathLengths[neighParent->id] + Dist(node->loc, neighParent->loc) - pathLengths[node->id];
            if (pathLengthDelta > slacks[node->id]) continue;
            auto steinerPt = GetNearestPoint(node, neigh);
            DTYPE wireLengthDelta = Dist(node->loc, steinerPt) - node->WireToParent();
            if (wireLengthDelta >= 0) continue;
            // break
            Disconnect(node);
            // reroot
            if (steinerPt == neigh->loc) {
                Connect(node, neigh);
            } else if (steinerPt == neighParent->loc) {
                Connect(node, neighParent);
            } else {
                auto steinerNode = make_shared<TreeNode>(steinerPt);
                Connect(steinerNode, neighParent);
                Disconnect(neigh);
                Connect(neigh, steinerNode);
                Connect(node, steinerNode);
                // for later moves
                steinerNode->id = nodes.size();
                nodes.push_back(steinerNode);
                pathLengths.push_back(pathLengths[neighParent->id] + steinerNode->WireToParent());
                slacks.push_back(Dist(steinerNode->loc, tree.source->loc) * (1 + eps) - pathLengths.back());
            }
            // update slack for later moves: first subtree, then path to source
            TreeNode::PreOrder(neighParent, UpdatePathLengths);
            TreeNode::PostOrder(neighParent, UpdateSlacks);
            auto tmp = neighParent;
            while (tmp->parent) {
                slacks[tmp->parent->id] = min(slacks[tmp->parent->id], slacks[tmp->id]);
                tmp = tmp->parent;
            }
        }

        // Finalize
        // tree.RemoveTopoRedundantSteiner();
        tree.PostOrderCopy([&](const shared_ptr<TreeNode>& node) {
            // degree may change after post-order traversal of its children
            if (node->pin) return;
            if (node->children.empty()) {
                Disconnect(node);
            } else if (node->children.size() == 1) {
                auto oldParent = node->parent, oldChild = node->children[0];
                Disconnect(node);
                Disconnect(oldChild);
                Connect(oldChild, oldParent);
            }
        });
    }
}

}  // namespace salt


#include <boost/multi_array.hpp>
#include <iostream>

// safety: array dimensions and data tied into one, can't mix them up!
// also: type safety.
void print(const boost::multi_array<double, 2>& array)
{
    for(int i=0; i<array.shape()[0]; i++) {
        for(int j=0; j<array.shape()[1]; j++) {
            std::cout << array[i][j] << " ";
        }
        std::cout << "\n";
    }

    std::cout << "\n";
}

int main(int argc, char **argv)
{
    int m = (argc>1) ? atoi(argv[1]) : 4;
    int n = (argc>2) ? atoi(argv[2]) : 5;

    boost::multi_array<double, 2> A(boost::extents[n][m]);
    for(int i=0; i<n; i++)
        for(int j=0; j<m; j++)
                A[i][j] = i * m + j;

    boost::multi_array<double, 2> B(boost::extents[n][m]);
    for(int i=0; i<n; i++)
        for(int j=0; j<m; j++)
                B[i][j] = i * m + j + 0.5;
    print(A);
    print(B);

    // safe copies, would even work with differing dimensions:
    B = A;
    print(B);

    // RAII: no manual free, no memory leaks
    return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Matrix2i a; a << 1, 2, 3, 4;
cout << "Here is the matrix a:\n" << a << endl;

a = a.transpose(); // !!! do NOT do this !!!
cout << "and the result of the aliasing effect:\n" << a << endl;

}
  return 0;
}


//
// Created by Samuel Jackson on 28/10/2015.
//

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <vector>

#include "mnist.h"
#include "../tools/BatchImageLoader.h"

const size_t BATCH_SIZE = 5;
const size_t NUM_CLASSES = 10;
const size_t INPUT_SIZE = 785;

int main(int argc, char** argv ) {
    NeuralNetwork ann = createNetwork();

    BatchImageLoader loader(argv[1], BATCH_SIZE, NUM_CLASSES);
    Eigen::MatrixXd examples;
    Eigen::MatrixXd labels;

    int batchCount = 0;
    while (loader.hasNext() && batchCount < 10) {
        std::cout << "BATCH " << batchCount << "===========================" << std::endl;

        loader.next();
        examples = loader.loadImages();
        labels = loader.loadLabels();

        ann.train(examples, labels, 1000, .1);
        ++batchCount;
    }

    Eigen::MatrixXd output = ann.feedForward(examples);

    std::cout << output << std::endl;
    std::cout << labels<< std::endl;

}

NeuralNetwork createNetwork() {
    int nodes[3] = {INPUT_SIZE, 50, NUM_CLASSES};
    std::vector<int> layout(&nodes[0], &nodes[0]+3);

    NeuralNetwork ann(layout, "sigmoid", false);
    return ann;
}


#pragma once

#include <cstdint>
#include <vector>

#include <Eigen/Eigen>

#include <apriltag.h>
#include <tagStandard41h12.h>
#include <apriltag_pose.h>
#include <opencv2/aruco/charuco.hpp>

#include <cereal/archives/json.hpp>
#include <open3d/geometry/PointCloud.h>

namespace cereal {
    template<class Archive>
    void serialize(Archive & archive,
                    Eigen::Quaternionf& m)
    {
        archive( cereal::make_nvp("x", m.x()), cereal::make_nvp("y", m.y()),
                    cereal::make_nvp("z", m.z()), cereal::make_nvp("w", m.w()) );
    };

    template<class Archive>
    void serialize(Archive & archive,
                    Eigen::Vector3f& t)
    {
        archive( cereal::make_nvp("m00", t.x()), cereal::make_nvp("m10", t.y()),
                    cereal::make_nvp("m20", t.z()) );
    };
}

struct  CameraExtrinsics
{
    Eigen::Vector3f translation;
    Eigen::Quaternionf rotation;

    template <class Archive>
    void serialize( Archive & ar )
    {
        ar(cereal::make_nvp("translation", translation), cereal::make_nvp("rotation", rotation));
    }

};

struct CameraIntrinsics
{
    // Sensor resolution
    int32_t Width, Height;

    // Intrinsics
    float cx, cy;
    float fx, fy;
    float k[6];
    float codx, cody;
    float p1, p2;
};

struct CameraCalibration
{
    // Intrinsics for each camera
    CameraIntrinsics Color, Depth;

    // Extrinsics transform from 3D depth camera point to 3D point relative to color camera
    float RotationFromDepth[3*3];
    float TranslationFromDepth[3];
};

//------------------------------------------------------------------------------
// Registration

struct FrameInfo
{
    CameraCalibration Calibration{};

    // Accelerometer reading for extrinsics calibration
    float Accelerometer[3];

    // Color image
    std::vector<uint8_t> ColorImage;
    int ColorWidth = 0, ColorHeight = 0, ColorStride = 0;

    // Depth image
    std::vector<uint16_t> DepthImage;
    int DepthWidth = 0, DepthHeight = 0, DepthStride = 0;

    // Point cloud data
    std::vector<int16_t> PointCloudData;

    uint32_t CameraIndex;
    int FrameNumber;
    char *filename;
};

struct AlignmentTransform
{
    float Transform[16];
    bool Identity = true;
    inline void operator=(const Eigen::Matrix4f& src)
    {
        Identity = src.isIdentity();
        for (int row = 0; row < 4; ++row) {
            for (int col = 0; col < 4; ++col) {
                Transform[row * 4 + col] = src(row, col);
            }
        }
    }
    inline void Set(Eigen::Matrix4f& dest) const
    {
        if (Identity) {
            dest = Eigen::Matrix4f::Identity();
        } else {
            for (int row = 0; row < 4; ++row) {
                for (int col = 0; col < 4; ++col) {
                    dest(row, col) = Transform[row * 4 + col];
                }
            }
        }
    }
};

class ExtrinsicsCalibration
{
    public:

        apriltag_family_t *tf;
        apriltag_detector_t* td;
        cv::Ptr<cv::aruco::Dictionary> dictionary;
        cv::Ptr<cv::aruco::CharucoBoard> board;
        cv::Ptr<cv::aruco::DetectorParameters> params;

        ExtrinsicsCalibration()
        {
            tf = tagStandard41h12_create();
            td = apriltag_detector_create();
            apriltag_detector_add_family_bits(td, tf, 1);
            td->quad_decimate = 1.f;
            td->quad_sigma = 0.8f;
            td->nthreads = 8;
            td->refine_edges = 1;
            td->decode_sharpening = 0.25;

            dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
            board = cv::aruco::CharucoBoard::create(5, 7, 0.04f, 0.02f, dictionary);
            params = cv::aruco::DetectorParameters::create();
        }

        ~ExtrinsicsCalibration()
        {
            tagStandard41h12_destroy(tf);
            apriltag_detector_destroy(td);
        }

        bool CalculateExtrinsics(
            const std::vector<FrameInfo>& frames,
            std::vector<AlignmentTransform>& extrinsics);

        bool RefineExtrinsics(
            const std::vector<FrameInfo>& frames,
            std::vector<AlignmentTransform>& extrinsics);
    private:
        std::vector<std::shared_ptr<open3d::geometry::PointCloud>> full_cloud;
        bool GenerateFullCloudFromFrames(FrameInfo frame);
        bool DownSample(std::shared_ptr<open3d::geometry::PointCloud>& cloud, const double voxel_size, const int idx);

};


#ifndef HSMM_H
#define HSMM_H

#include <armadillo>
#include <deque>
#include <emissions.hpp>
#include <ForwardBackward.hpp>
#include <json.hpp>
#include <memory>
#include <vector>

namespace hsmm {

    class HSMM {
        public:
            HSMM(std::shared_ptr<AbstractEmission> emission,
                    arma::mat transition, arma::vec pi, arma::mat duration,
                    int min_duration);

            // Inits all the pmfs with the uniform distribution.
            HSMM(std::shared_ptr<AbstractEmission> emission, int nstates,
                    int ndurations, int min_duration);

            void setDuration(arma::mat duration);

            void setEmission(std::shared_ptr<AbstractEmission> emission);

            void setPi(arma::vec pi);

            void setTransition(arma::mat transition);

            void setDurationLearningChoice(std::string choice);

            void setDurationDirichletPrior(arma::mat alphas);

            void setPiFromLabels(arma::field<arma::ivec> seqs);

            void setTransitionFromLabels(arma::field<arma::ivec> seqs);

            void setDurationFromLabels(arma::field<arma::ivec> seqs);

            arma::field<arma::mat> sampleSegments(int nsegments,
                    arma::ivec& hiddenStates,
                    arma::ivec& hiddenDurations);

            arma::field<arma::field<arma::mat>> sampleMultipleSequences(
                    int nsequences, int nsegments,
                    arma::field<arma::ivec>& seqsHiddenStates,
                    arma::field<arma::ivec>& seqsHiddenDurations);

            void init_params_from_data(const arma::field<arma::field<
                    arma::mat>> &obs);

            // Fits the model from multiple sequences.
            bool fit(arma::field<arma::field<arma::mat>> obs,
                    int max_iter, double tol);

            // As above (multiple sequences) but also accepts labels for
            // semi-supervised learning.
            bool fit(arma::field<arma::field<arma::mat>> obs,
                    arma::field<Labels> observed_segments, int max_iter,
                    double tol);

            double loglikelihood(const arma::field<arma::field<
                    arma::mat>>& obs);

            double loglikelihood(
                    const arma::field<arma::field<arma::mat>>& obs,
                    const arma::field<Labels> &observed_segments);

            // Computes the likelihoods w.r.t. the emission model.
            arma::cube computeEmissionsLikelihood(
                    const arma::field<arma::mat>& obs);

            // Computes the loglikelihoods w.r.t. the emission model.
            arma::cube computeEmissionsLogLikelihood(
                    const arma::field<arma::mat>& obs);

            // Returns a json representation of the model.
            nlohmann::json to_stream() const;

            // Reads the parameters from a json file.
            void from_stream(const nlohmann::json &params);

            arma::mat transition_;
            arma::vec pi_;
            arma::mat duration_;
            int ndurations_;
            int min_duration_;
            int nstates_;

            // Parameters of the Dirichlet prior assumed over the duration.
            // By default no prior is assumed \alpha = 1.
            arma::mat dirichlet_alphas_;

            std::shared_ptr<AbstractEmission> emission_;
            std::string duration_learning_choice_;
            bool learning_transitions_;
            bool learning_pi_;
            bool debug_;

        protected:

            std::pair<arma::ivec, arma::ivec>
                    computeViterbiStateDurationSequenceFromLabels(
                    arma::ivec seq);

            // Approximates a discrete (truncate) Gaussian to a given duration.
            arma::mat gaussianMomentMatching(arma::mat duration) const;

            double lower_bound_term_transition(const arma::field<
                    arma::cube> &zetas, const arma::mat& log_transition) const;

            double lower_bound_term_pi(const arma::field<arma::cube> &etas,
                    const arma::vec& log_pi) const;

            double lower_bound_term_duration(const arma::field<
                    arma::cube> &etas, const arma::mat& log_duration) const;
    };

    class OnlineHSMM : public HSMM {
        public:
            OnlineHSMM(std::shared_ptr<AbstractEmissionOnlineSetting> emission,
                    arma::mat transition, arma::vec pi, arma::mat duration,
                    int min_duration);

            std::shared_ptr<AbstractEmissionOnlineSetting> getOnlineEmission(
                    ) const;

            void addNewObservation(const arma::mat& obs);

            void sampleFromPosterior(int &dur, int &offset, int &hs) const;

            arma::field<arma::mat> sampleNextObservations(int nobs) const;

            void printTopKFromPosterior(int k) const;

            arma::vec getStateMarginal() const;

            arma::vec getRunlengthMarginal() const;

            arma::vec getDurationMarginal() const;

            arma::vec getResidualTimeMarginal() const;

            arma::vec getImplicitResidualTimeMarginal() const;

            arma::vec getImplicitDurationMarginal() const;

            double getLastOneStepAheadLoglikelihood() const;

        protected:

            arma::mat getDurationSuffixSum() const;

            int appendToField(arma::field<arma::mat>& current_obs, int idx,
                    const arma::field<arma::mat> new_obs) const;

            // This represents p(y_{t+1} | y_1, y_2, ..., y_t).
            double last_normalization_c_;

            // Logposterior over (d (duration),s (offset),i (hidden state))
            // conditioned on all the observations given so far.
            arma::cube last_log_posterior_;
            std::vector<arma::mat> observations_;
            std::deque<arma::vec> alpha_posteriors_;
    };

    class OnlineHSMMRunlengthBased : public HSMM {
        public:
            OnlineHSMMRunlengthBased(std::shared_ptr<
                    AbstractEmissionObsCondIIDgivenState> emission,
                    arma::mat transition, arma::vec pi, arma::mat duration,
                    int min_duration);

            OnlineHSMMRunlengthBased(std::shared_ptr<
                    AbstractEmissionObsCondIIDgivenState> emission,
                    int nstates, int ndurations, int min_duration);

            void init();

            std::shared_ptr<AbstractEmissionObsCondIIDgivenState>
                    getOnlineDurationAgnosticEmission() const;

            void addNewObservation(const arma::mat& obs);

            arma::vec getRunlengthMarginal() const;

            arma::vec getStateMarginal() const;

            arma::vec getResidualTimeMarginal() const;

            // Based on last_residualtime_posterior_
            arma::vec getStateMarginal2() const;

            double oneStepAheadLoglikelihood(const arma::mat& obs) const;

            double oneStepAheadLoglikelihood2(const arma::mat& obs) const;

        protected:

            double loglikelihood_(int state, const arma::mat& obs) const;

            arma::mat getDurationSuffixSum_() const;

            // Hazard(r): probability that the segment ends "now" given that
            // the current runlength is r.
            arma::mat getHazardFunction_() const;

            // Posterior over (r (runlength a.k.a. offset), i (hidden state))
            arma::mat last_posterior_;

            // Posterior over (l (residual time), i, (hidden state))
            arma::mat last_residualtime_posterior_;

            std::vector<arma::mat> observations_;
    };

};

#endif


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::models::exponential::include.hpp             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_SURVIVAL_MODEL_MODELS_EXPONENTIAL_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_MODEL_MODELS_EXPONENTIAL_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/model/models/exponential/model.hpp>
#include <boost/statistics/survival/model/models/exponential/log_likelihood.hpp>
#include <boost/statistics/survival/model/models/exponential/meta_failure_distribution.hpp>

#endif


#include "ut_common.hpp"
#include <gauxc/xc_integrator.hpp>
#include <gauxc/xc_integrator/impl.hpp>

#include <Eigen/Core>

using namespace GauXC;

#ifdef GAUXC_ENABLE_MPI
void test_xc_integrator( ExecutionSpace ex, MPI_Comm comm, Molecule mol, const bool check_state_propagation = true )
#else
void test_xc_integrator( ExecutionSpace ex, Molecule mol, const bool check_state_propagation = true )
#endif
{

  BasisSet<double> basis = make_ccpvdz( mol, SphericalType(true) );

  for( auto& sh : basis )
    sh.set_shell_tolerance( std::numeric_limits<double>::epsilon() );
  basis.generate_shell_to_ao();

  MolGrid mg(AtomicGridSizeDefault::UltraFineGrid, mol);

  auto meta = std::make_shared<MolMeta>( mol );

#ifdef GAUXC_ENABLE_MPI
  auto lb = std::make_shared<LoadBalancer>(comm, mol, mg, basis, meta);
#else
  auto lb = std::make_shared<LoadBalancer>(mol, mg, basis, meta);
#endif

  functional_type func( ExchCXX::Backend::builtin, ExchCXX::XCFunctional::Functional::PBE0, ExchCXX::Spin::Unpolarized );

  using matrix_type = Eigen::MatrixXd;
#ifdef GAUXC_ENABLE_MPI
  XCIntegrator<matrix_type> integrator( ex, comm, func, basis, lb );
#else
  XCIntegrator<matrix_type> integrator( ex, func, basis, lb );
#endif

  matrix_type P,VXC_ref;
  double EXC_ref;

  {
    std::string ref_file = GAUXC_REF_DATA_PATH "/benzene_pbe0_cc-pvdz_ufg_ssf.bin";
    std::ifstream infile( ref_file, std::ios::binary );
    cereal::BinaryInputArchive ar(infile);
    ar( EXC_ref, P, VXC_ref );
  }

  auto [ EXC, VXC ] = integrator.eval_exc_vxc( P );
  CHECK( EXC == Approx( EXC_ref ) );

  //std::cout << "VXC" << std::endl;
  //std::cout << VXC << std::endl;
  //std::cout << "VXC_ref" << std::endl;
  //std::cout << VXC_ref << std::endl;
  //std::cout << "DIFF" << std::endl;
  //std::cout << (VXC-VXC_ref) << std::endl;

  auto VXC_diff_nrm = ( VXC - VXC_ref ).norm();
  CHECK( VXC_diff_nrm / basis.nbf() < 1e-10 );

  // Check if the integrator propagates state correctly
  if( check_state_propagation ) {
    auto [ EXC1, VXC1 ] = integrator.eval_exc_vxc( P );
    CHECK( EXC1 == Approx( EXC_ref ) );
    auto VXC1_diff_nrm = ( VXC1 - VXC_ref ).norm();
    CHECK( VXC1_diff_nrm / basis.nbf() < 1e-10 );
  }
}

TEST_CASE( "Benzene / PBE0 / cc-pVDZ", "[xc-integrator]" ) {

#ifdef GAUXC_ENABLE_MPI
  MPI_Comm comm = MPI_COMM_WORLD;
#endif
  Molecule mol  = make_benzene();

#ifdef GAUXC_ENABLE_HOST
  SECTION( "Host" ) {
#ifdef GAUXC_ENABLE_MPI
    test_xc_integrator( ExecutionSpace::Host, comm, mol );
#else
    test_xc_integrator( ExecutionSpace::Host, mol );
#endif
  }
#endif

#ifdef GAUXC_ENABLE_CUDA
  SECTION( "Device" ) {
#ifdef GAUXC_ENABLE_MPI
    test_xc_integrator( ExecutionSpace::Device, comm, mol );
#else
    test_xc_integrator( ExecutionSpace::Device, mol );
#endif
  }
#endif

}


#pragma once

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationProblem/kinematicallyRedundantMachines.hpp"

namespace mant {
  namespace krm {
    class ParallelKinematicMachine3PRRR : public KinematicallyRedundantMachines {
     public:
      explicit ParallelKinematicMachine3PRRR();

     protected:
      const arma::mat::fixed<2, 3> redundantJointsPosition_;
      const arma::mat::fixed<2, 3> redundantJointsAngles_;
      const arma::mat::fixed<2, 3> linkLengths_;
      const arma::mat::fixed<2, 3> endEffectorJointsRelativePosition_;
    };
  }
}


#ifndef TM_KIT_TRANSPORT_BCL_COMPAT_DECIMAL_HPP_
#define TM_KIT_TRANSPORT_BCL_COMPAT_DECIMAL_HPP_

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

namespace dev { namespace cd606 { namespace tm { namespace transport { namespace bcl_compat {
    class DecimalConverter {
    public:
        template <class BclDecimalProtoType>
        static boost::multiprecision::cpp_dec_float_100 read(BclDecimalProtoType const &d) {
            uint64_t lo = d.lo();
            uint32_t hi = d.hi();
            boost::multiprecision::int128_t base = hi;
            base = base << 64;
            base = base | boost::multiprecision::int128_t(lo);
            uint32_t signScale = d.signscale();
            uint32_t scale = (signScale >> 1);
            boost::multiprecision::cpp_dec_float_100 ret(base);
            ret /= boost::multiprecision::pow(boost::multiprecision::cpp_dec_float_100(10), scale);
            if ((signScale & 1) == 0) {
                return ret;
            } else {
                return -ret;
            }
        }
        template <class BclDecimalProtoType>
        static void write(BclDecimalProtoType &d, boost::multiprecision::cpp_dec_float_100 const &input) {
            bool isNeg = (input < 0);
            uint32_t exp = 0;
            boost::multiprecision::cpp_dec_float_100 x = (isNeg?-input:input);
            boost::multiprecision::int128_t intRep = x.convert_to<boost::multiprecision::int128_t>();
            for (; exp<100; ++exp) {
                if (x == intRep.convert_to<boost::multiprecision::cpp_dec_float_100>()) {
                    break;
                }
                x = x*10;
                intRep = x.convert_to<boost::multiprecision::int128_t>();
            }
            d.set_lo((uint64_t) (intRep & (uint64_t) 0xffffffffffffffff));
            d.set_hi((uint32_t) ((intRep >> 64) && (uint32_t) 0xffffffff));
            d.set_signscale(
                (exp << 1) | (isNeg?1:0)
            );
        }
    };

}}}}}

#endif


#include "gflags/gflags.h"
#include <iostream>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/filesystem.hpp>
#include "tbb/tbb.h"
#include "tbb/concurrent_vector.h"
#include "bakuage/sound_quality2.h"

DECLARE_string(input);
DECLARE_string(analysis_data_dir);
DECLARE_string(sound_quality2_cache);
DECLARE_string(sound_quality2_cache_archiver);

namespace {
    struct Item {
        std::string path;
        double distance;
    };
}

// 学習済みのcacheを使って距離を計算する (前処理などを利用するために、学習済みのcacheが必要)
void SoundQuality2FindNn() {
    using boost::filesystem::recursive_directory_iterator;

    bakuage::SoundQuality2Calculator calculator;
    {
        if (FLAGS_sound_quality2_cache_archiver == "binary") {
			std::ifstream ifs(FLAGS_sound_quality2_cache, std::ios::binary);
            boost::archive::binary_iarchive ia(ifs);
            ia >> calculator;
        } else if (FLAGS_sound_quality2_cache_archiver == "text") {
			std::ifstream ifs(FLAGS_sound_quality2_cache);
            boost::archive::text_iarchive ia(ifs);
            ia >> calculator;
        } else {
            throw std::logic_error("unknown archive type " + FLAGS_sound_quality2_cache_archiver);
        }
    }

    Eigen::VectorXd mean;
    Eigen::MatrixXd cov;
    bakuage::SoundQuality2CalculatorUnit::ParseReference(bakuage::LoadStrFromFile(FLAGS_input.c_str()).c_str(), &mean, &cov);
    bakuage::MasteringReference2 target(mean, cov);

    tbb::concurrent_vector<Item> items;

    recursive_directory_iterator last;
    std::vector<std::string> paths;
    for (recursive_directory_iterator itr(FLAGS_analysis_data_dir); itr != last; ++itr) {
        const std::string path = itr->path().string();
        if (!bakuage::StrEndsWith(path, ".json")) continue;
        paths.emplace_back(path);
    }

    tbb::parallel_for<int>(0, paths.size(), [&paths, &items, &calculator, &target](int i) {
        Eigen::VectorXd mean;
        Eigen::MatrixXd cov;
        bakuage::SoundQuality2CalculatorUnit::ParseReference(bakuage::LoadStrFromFile(paths[i].c_str()).c_str(), &mean, &cov);
        bakuage::MasteringReference2 reference(mean, cov);
        Item item;
        item.path = paths[i];
        item.distance = calculator.CalculateDistance(reference, target);
        items.emplace_back(item);
    });

    std::sort(items.begin(), items.end(), [](const Item &a, const Item &b) {
        return a.distance < b.distance;
    });

    for (int i = 0; i < std::min<int>(100, items.size()); i++) {
        std::cout << "distance:" << items[i].distance << "\tpath:" << items[i].path << std::endl;
    }
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// check memory allocation in some method

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_MODULE Statics
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include "RBDyn/Body.h"
#include "RBDyn/FD.h"
#include "RBDyn/FK.h"
#include "RBDyn/FV.h"
#include "RBDyn/IS.h"
#include "RBDyn/Joint.h"
#include "RBDyn/MultiBody.h"
#include "RBDyn/MultiBodyConfig.h"
#include "RBDyn/MultiBodyGraph.h"
#include "RBDyn/util.hh"
#include "fixture.hh"

// arm
#include "XXXarm.h"

namespace rbd
{
using namespace Eigen;
Eigen::IOFormat cleanFmt(2, 0, ", ", "\n", "[", "]");

static constexpr double PI = boost::math::constants::pi<double>();

void test(boost::shared_ptr<boost::test_tools::output_test_stream> output,
          rbd::MultiBody & mb,
          rbd::MultiBodyConfig & mbc,
          rbd::InverseStatics & IS,
          Eigen::Vector3d q)
{
  Eigen::MatrixXd jacQ(3, 3);
  Eigen::MatrixXd jacF(3, 24);
  std::vector<Eigen::MatrixXd> jacMomentAndForces(4);
  (*output) << "\n\n\nChange config to " << q.transpose() << std::endl;
  mbc.q[1][0] = q(0);
  mbc.q[2][0] = q(1);
  mbc.q[3][0] = q(2);
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);
  IS.inverseStatics(mb, mbc);
  IS.computeTorqueJacobianJoint(mb, mbc, jacMomentAndForces);

  (*output) << "========================================" << std::endl;
  (*output) << "Results for mbc.q =" << mbc.q << std::endl;
  (*output) << "mbc.jointTorque =\n" << mbc.jointTorque << std::endl;
  for(auto e : IS.f()) (*output) << "IS.f().vector =\n" << e.vector() << std::endl;
  (*output) << "IS.jointTorqueDiff =\n" << IS.jointTorqueDiff() << std::endl;
}

BOOST_AUTO_TEST_CASE(XXXArmTorqueJacobian)
{
  boost::shared_ptr<boost::test_tools::output_test_stream> output = retrievePattern("InverseStaticsTest");

  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXXXarm();
  rbd::InverseStatics IS(mb);

  test(output, mb, mbc, IS, Vector3d(0, 0, 0));
  test(output, mb, mbc, IS, Vector3d(rbd::PI, 0, 0));
  test(output, mb, mbc, IS, Vector3d(0, rbd::PI / 2, 0));
  test(output, mb, mbc, IS, Vector3d(0.4, 0.1, 0.2));

  std::cout << output->str() << std::endl;
#ifndef __i386__
  BOOST_CHECK(output->match_pattern());
#endif
} // end of namespace rbd
} // namespace rbd


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_RANDOM_ALGEBRAIC_RANDOM_DEVICE_HPP
#define CRYPTO3_RANDOM_ALGEBRAIC_RANDOM_DEVICE_HPP

#include <type_traits>

#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <nil/crypto3/algebra/type_traits.hpp>

namespace nil {
    namespace crypto3 {
        namespace random {
            /*!
             * @brief
             * @tparam AlgebraicType denote an some algebraic type (field, curve group types).
             *
             * algebraic_random_device is adapter wrapping boost::random_device producing random values of algebraic
             * type.
             *
             * The class template algebraic_random_device models a \UniformRandomBitGenerator.
             * https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator
             */
            template<typename AlgebraicType, typename = void>
            struct algebraic_random_device;

            template<typename AlgebraicType>
            struct algebraic_random_device<
                AlgebraicType, typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                                       !algebra::is_extended_field<AlgebraicType>::value>::type> {
            protected:
                typedef AlgebraicType field_type;
                typedef typename field_type::value_type field_value_type;
                typedef typename field_type::integral_type integral_type;

                typedef boost::random_device internal_generator_type;
                typedef boost::random::uniform_int_distribution<integral_type> internal_distribution_type;

                constexpr static integral_type _min = 0;
                constexpr static integral_type _max = field_type::modulus - 1;

            public:
                typedef field_value_type result_type;

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    return dist(gen);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type min() {
                    constexpr result_type min_value(_min);
                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type max() {
                    constexpr result_type max_value(_max);
                    return max_value;
                }

            protected:
                internal_generator_type gen;
                internal_distribution_type dist = internal_distribution_type(_min, _max);
            };

            template<typename AlgebraicType>
            struct algebraic_random_device<
                AlgebraicType, typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                                       algebra::is_extended_field<AlgebraicType>::value>::type> {
            protected:
                typedef AlgebraicType extended_field_type;
                typedef typename extended_field_type::value_type extended_field_value_type;
                typedef typename extended_field_type::underlying_field_type underlying_field_type;

                typedef algebraic_random_device<underlying_field_type> internal_generator_type;

            public:
                typedef extended_field_value_type result_type;

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    result_type result;
                    for (auto &coord : result.data) {
                        coord = gen();
                    }

                    return result;
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate min_value at compile-time
                constexpr static inline result_type min() {
                    result_type min_value;
                    for (auto &coord : min_value.data) {
                        coord = internal_generator_type::min();
                    }

                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate max_value at compile-time
                constexpr static inline result_type max() {
                    result_type max_value;
                    for (auto &coord : max_value.data) {
                        coord = internal_generator_type::max();
                    }

                    return max_value;
                }

            protected:
                internal_generator_type gen;
            };

            template<typename AlgebraicType>
            struct algebraic_random_device<
                AlgebraicType, typename std::enable_if<algebra::is_curve_group<AlgebraicType>::value>::type> {
            protected:
                typedef AlgebraicType group_type;
                typedef typename group_type::value_type group_value_type;
                typedef typename group_type::curve_type::scalar_field_type scalar_field_type;

                typedef algebraic_random_device<scalar_field_type> internal_generator_type;

            public:
                typedef group_value_type result_type;

                /**
                 * Returns a random value in the range [min, max]. Elements of group are ordered in exponent growing
                 * order with respect to group base element.
                 */
                // TODO: check correctness of the generation method
                result_type operator()() {
                    return result_type::one() * gen();
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate returned value at compile-time
                constexpr static inline result_type min() {
                    return result_type::zero();
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate returned value at compile-time
                constexpr static inline result_type max() {
                    return result_type::one() * (scalar_field_type::modulus - 1);
                }

            protected:
                internal_generator_type gen;
            };
        }    // namespace random
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_RANDOM_ALGEBRAIC_RANDOM_DEVICE_HPP


/*
 * GridMapMath.hpp
 *
 *  Created on: Dec 2, 2013
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include <Eigen/Core>
#include <vector>
#include <map>

namespace grid_map_lib {

/*!
 * Gets the position of a cell specified by its index in the map frame.
 * @param[out] position the position of the center of the cell in the map frame.
 * @param[in] index of the cell.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the size of the buffer (optional).
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if index not within range of buffer.
 */
bool getPositionFromIndex(Eigen::Vector2d& position,
                          const Eigen::Array2i& index,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Gets the index of the cell which contains a position in the map frame.
 * @param[out] index of the cell.
 * @param[in] position the position in the map frame.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the size of the buffer (optional).
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if position outside of map.
 */
bool getIndexFromPosition(Eigen::Array2i& index,
                          const Eigen::Vector2d& position,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Checks if position is within the map boundaries.
 * @param[in] position the position which is to be checked.
 * @param[in] mapLength the length of the map.
 * @param[in] mapPosition the position of the map.
 * @return true if position is within map, false otherwise.
 */
bool checkIfPositionWithinMap(const Eigen::Vector2d& position,
                              const Eigen::Array2d& mapLength,
                              const Eigen::Vector2d& mapPosition);

/*!
 * Gets the position of the data structure origin.
 * @param[in] position the position of the map.
 * @param[in] mapLength the map length.
 * @param[out] positionOfOrigin the position of the data structure origin.
 */
void getPositionOfDataStructureOrigin(const Eigen::Vector2d& position,
                                      const Eigen::Array2d& mapLength,
                                      Eigen::Vector2d& positionOfOrigin);

/*!
 * Computes how many cells/indeces the map is moved based on a position shift in
 * the grid map frame. Use this function if you are moving the grid map
 * and want to ensure that the cells match before and after.
 * @param[out] indexShift the corresponding shift of the indices.
 * @param[in] positionShift the desired position shift.
 * @param[in] resolution the resolution of the map.
 * @return true if successful.
 */
bool getIndexShiftFromPositionShift(Eigen::Array2i& indexShift,
                                    const Eigen::Vector2d& positionShift,
                                    const double& resolution);

/*!
 * Computes the corresponding position shift from a index shift. Use this function
 * if you are moving the grid map and want to ensure that the cells match
 * before and after.
 * @param[out] positionShift the corresponding shift in position in the grid map frame.
 * @param[in] indexShift the desired shift of the indeces.
 * @param[in] resolution the resolution of the map.
 * @return true if successful.
 */
bool getPositionShiftFromIndexShift(Eigen::Vector2d& positionShift,
                                    const Eigen::Array2i& indexShift,
                                    const double& resolution);

/*!
 * Checks if index is within range of the buffer.
 * @param[in] index to check.
 * @param[in] bufferSize the size of the buffer.
 * @return true if index is within, and false if index is outside of the buffer.
 */
bool checkIfIndexWithinRange(const Eigen::Array2i& index, const Eigen::Array2i& bufferSize);

/*!
 * Maps an index that runs out of the range of the circular buffer back into allowed the region.
 * This is the 2d version of mapIndexWithinRange(int&, const int&).
 * @param[in/out] index the indeces that will be mapped into the valid region of the buffer.
 * @param[in] bufferSize the size of the buffer.
 */
void mapIndexWithinRange(Eigen::Array2i& index,
                         const Eigen::Array2i& bufferSize);

/*!
 * Maps an index that runs out of the range of the circular buffer back into allowed the region.
 * @param[in/out] index the index that will be mapped into the valid region of the buffer.
 * @param[in] bufferSize the size of the buffer.
 */
void mapIndexWithinRange(int& index, const int& bufferSize);

/*!
 * Limits (cuts off) the position to lie inside the map.
 * @param[in/out] position the position to be limited.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 */
void limitPositionToRange(Eigen::Vector2d& position,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition);

/*!
 * Provides the alignment transformation from the buffer order (outer/inner storage)
 * and the map frame (x/y-coordinate).
 * @return the alignment transformation.
 */
const Eigen::Matrix2i getBufferOrderToMapFrameAlignment();

/*!
 * Given a map and a desired submap (defined by position and size), this function computes
 * various information about the submap. The returned submap might be smaller than the requested
 * size as it respects the boundaries of the map.
 * @param[out] submapTopLeftIndex the top left index of the returned submap.
 * @param[out] submapBufferSize the buffer size of the returned submap.
 * @param[out] submapPosition the position of the submap (center) in the map frame.
 * @param[out] submapLength the length of the submap.
 * @param[out] requestedIndexInSubmap the index in the submap that corresponds to the requested
 *             position of the submap.
 * @param[in] requestedSubmapPosition the requested submap position (center) in the map frame.
 * @param[in] requestedSubmapLength the requested submap length.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the buffer size of the map.
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful.
 */
bool getSubmapInformation(Eigen::Array2i& submapTopLeftIndex,
                          Eigen::Array2i& submapBufferSize,
                          Eigen::Vector2d& submapPosition,
                          Eigen::Array2d& submapLength,
                          Eigen::Array2i& requestedIndexInSubmap,
                          const Eigen::Vector2d& requestedSubmapPosition,
                          const Eigen::Vector2d& requestedSubmapLength,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Computes the regions in the circular buffer that make up the data for
 * a requested submap.
 * @param[out] submapIndeces the list of indeces (top-left) for the buffer regions.
 * @param[out] submapSizes the sizes of the buffer regions.
 * @param[in] submapIndex the index (top-left) for the requested submap.
 * @param[in] submapBufferSize the size of the requested submap.
 * @param[in] bufferSize the buffer size of the map.
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if requested submap is not fully contained in the map.
 */
bool getBufferRegionsForSubmap(std::vector<Eigen::Array2i>& submapIndeces,
                               std::vector<Eigen::Array2i>& submapSizes,
                               const Eigen::Array2i& submapIndex,
                               const Eigen::Array2i& submapBufferSize,
                               const Eigen::Array2i& bufferSize,
                               const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Increases the index by one to iterate through the map.
 * Increments either to the neighboring index to the right or to
 * the start of the lower row. Returns false if end of iterations are reached.
 * @param[in/out] index the index in the map that is incremented (corrected for the circular buffer).
 * @param[in] bufferSize the map buffer size.
 * @param[in] bufferStartIndex the map buffer start index.
 * @return true if successfully incremented indeces, false if end of iteration limits are reached.
 */
bool incrementIndex(Eigen::Array2i& index, const Eigen::Array2i& bufferSize,
                    const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Increases the index by one to iterate through the cells of a submap.
 * Increments either to the neighboring index to the right or to
 * the start of the lower row. Returns false if end of iterations are reached.
 *
 * Note: This function does not check if submap actually fits to the map. This needs
 * to be checked before separately.
 *
 * @param[in/out] submapIndex the index in the submap that is incremented.
 * @param[out] index the index in the map that is incremented (corrected for the circular buffer).
 * @param[in] submapTopLefIndex the top left index of the submap.
 * @param[in] submapBufferSize the submap buffer size.
 * @param[in] bufferSize the map buffer size.
 * @param[in] bufferStartIndex the map buffer start index.
 * @return true if successfully incremented indeces, false if end of iteration limits are reached.
 */
bool incrementIndexForSubmap(Eigen::Array2i& submapIndex, Eigen::Array2i& index, const Eigen::Array2i& submapTopLeftIndex,
                             const Eigen::Array2i& submapBufferSize, const Eigen::Array2i& bufferSize,
                             const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Returns the 1d index corresponding to the 2d index for either row- or column-major format.
 * Note: Eigen is defaulting to column-major format.
 * @param[in] index the 2d index.
 * @param[in] bufferSize the map buffer size.
 * @param[in] (optional) rowMajor if the 1d index is generated for row-major format.
 * @return the 1d index.
 */
unsigned int get1dIndexFrom2dIndex(const Eigen::Array2i& index, const Eigen::Array2i& bufferSize, const bool rowMajor);

/*!
 * The definition of the buffer regions.
 */
enum class BufferRegion
{
  TopLeft,
  TopRight,
  BottomLeft,
  BottomRight
};

/*!
 * The definition of the position in the list for the buffer regions.
 */
static std::map<BufferRegion, int> bufferRegionIndeces =
{
{ BufferRegion::TopLeft, 0 },
{ BufferRegion::TopRight, 1 },
{ BufferRegion::BottomLeft, 2 },
{ BufferRegion::BottomRight, 3 } };

} // namespace


#define BOOST_TEST_MODULE ransac_solver_unit_tests

#include <boost/test/included/unit_test.hpp>
#include <armadillo>
#include "../src/analytical/linear/ls_solver.h"
#include "../src/robust/ransac_solver.cpp"
#include "test_utils.cpp"
#include "../src/logging/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

using namespace arma;

BOOST_AUTO_TEST_CASE(fit_random_signals_with_outliers)
{
    mat weights = {-10, 1, 500};
    mat signals = arma::randu(3, 100);
    mat signal = sum_signal(weights, signals);

    // Add outliers to signal
    std::vector<int> outlier_indices = {1, 5, 10, 35, 67, 85, 98};
    std::default_random_engine generator;
    std::normal_distribution<double> distribution(0, 10);
    for (auto &&i : outlier_indices)
    {
        signal[i] += distribution(generator);
    }

    LSSolver solver = LSSolver(signals);
    std::shared_ptr<LSSolver> solver_ptr(new LSSolver(solver));
    auto ransac_solver = RansacSolver(solver_ptr, 5, 0.1, 80);

    // With regular non-robust solver, we shouldn't get corrent solution
    mat result = solver.solve(signal);
    BOOST_CHECK(!is_equal(weights, result));

    // RANSAC solver should handle outliers
    mat result_ransac = ransac_solver.solve(signal);
    BOOST_CHECK(is_equal(weights, result_ransac));
}

BOOST_AUTO_TEST_CASE(solver_should_produce_same_result_with_different_constructor_methods)
{

    mat weights = {-10, 1, 500};
    mat signals = arma::randu(3, 100);
    mat signal = sum_signal(weights, signals);

    LSSolver solver = LSSolver(signals);
    std::shared_ptr<LSSolver> solver_ptr(new LSSolver(solver));
    auto ransac_solver1 = RansacSolver(solver_ptr, 5, 0.1, 80);

    auto ransac_solver2 = RansacSolver(signals, 5, 0.1, 80);

    mat result1 = ransac_solver1.solve(signal);
    mat result2 = ransac_solver2.solve(signal);

    BOOST_CHECK(is_equal(result1, result2));
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_RMSPROP_HPP
#define NETKET_RMSPROP_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_stepper.hpp"

namespace netket {

class RMSProp : public AbstractStepper {
  int npar_;

  double eta_;
  double beta_;

  Eigen::VectorXd st_;

  double epscut_;

  int mynode_;

  const std::complex<double> I_;

 public:
  // Json constructor
  explicit RMSProp(const json &pars)
      : eta_(FieldOrDefaultVal(pars["Learning"],"LearningRate",0.001)),
        beta_(FieldOrDefaultVal(pars["Learning"],"Beta",0.9)),
        epscut_(FieldOrDefaultVal(pars["Learning"],"Epscut",1.0e-8)),
        I_(0, 1) {
    npar_ = -1;

    PrintParameters();
  }

  void PrintParameters() {
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);
    if (mynode_ == 0) {
      std::cout << "# RMSProp stepper initialized with these parameters : "
                << std::endl;
      std::cout << "# Learning Rate = " << eta_ << std::endl;
      std::cout << "# Beta = " << beta_ << std::endl;
      std::cout << "# Epscut = " << epscut_ << std::endl;
    }
  }

  void Init(const Eigen::VectorXd &pars) override {
    npar_ = pars.size();
    st_.setZero(npar_);
  }

  void Init(const Eigen::VectorXcd &pars) override {
    npar_ = 2 * pars.size();
    st_.setZero(npar_);
  }

  void Update(const Eigen::VectorXd &grad, Eigen::VectorXd &pars) override {
    assert(npar_ > 0);

    st_=beta_*st_+(1.-beta_)*grad.cwiseAbs2();

    for(int i=0;i<npar_;i++){
      pars(i)-=eta_*grad(i)/(std::sqrt(st_(i))+epscut_);
    }
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXd &pars) override {
    Update(Eigen::VectorXd(grad.real()), pars);
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXcd &pars) override {
    assert(npar_ == 2 * pars.size());

    for(int i=0;i<pars.size();i++){
      st_(2*i)=beta_*st_(2*i)+(1.-beta_)*std::pow(grad(i).real(),2);
      st_(2*i+1)=beta_*st_(2*i+1)+(1.-beta_)*std::pow(grad(i).imag(),2);
      pars(i)-=eta_*grad(i).real()/(std::sqrt(st_(2*i))+epscut_);
      pars(i)-=eta_*I_*grad(i).imag()/(std::sqrt(st_(2*i+1))+epscut_);
    }
  }

  void Reset() override {
    st_ = Eigen::VectorXd::Zero(npar_);
  }

};

}  // namespace netket

#endif