// Copyright 2018 Xiaomi, 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.

#include <Eigen/Dense>
#include <algorithm>
#include <string>
#include <vector>

#include "mace/core/testing/test_benchmark.h"
#include "mace/kernels/gemm.h"

namespace mace {
namespace kernels {
namespace test {

// Test the speed of different access order of a NHWC buffer

namespace {

// Matmul with (m, k) x (k, n)
void MatmulBenchmark_Mace(int iters, int m, int k, int n) {
  mace::testing::StopTiming();
  std::vector<float> lhs(m * k);
  std::vector<float> rhs(k * n);
  std::vector<float> result(m * n);
  // warm up
  Gemm(lhs.data(), rhs.data(), 1, m, k, n, result.data());
  mace::testing::StartTiming();
  while (iters--) {
    Gemm(lhs.data(), rhs.data(), 1, m, k, n, result.data());
  }
}

void MatmulBenchmark_Eigen(int iters, int m, int k, int n) {
  mace::testing::StopTiming();
  Eigen::MatrixXd lhs = Eigen::MatrixXd::Random(m, k);
  Eigen::MatrixXd rhs = Eigen::MatrixXd::Random(k, n);
  Eigen::MatrixXd result = Eigen::MatrixXd::Zero(m, n);
  // warm up
  result = lhs * rhs;
  mace::testing::StartTiming();
  while (iters--) {
    result = lhs * rhs;
  }
}

}  // namespace

#define MACE_BM_MATMUL_FUNC(M, K, N, FUNC)                         \
  static void MACE_BM_MATMUL_##M##_##K##_##N##_##FUNC(int iters) { \
    const int64_t macc = static_cast<int64_t>(iters) * M * K * N;  \
    const int64_t tot = static_cast<int64_t>(iters) * (M + N) * K; \
    mace::testing::MaccProcessed(macc);                            \
    mace::testing::BytesProcessed(tot * sizeof(float));            \
    MatmulBenchmark_##FUNC(iters, M, K, N);                        \
  }                                                                \
  MACE_BENCHMARK(MACE_BM_MATMUL_##M##_##K##_##N##_##FUNC)

#define MACE_BM_MATMUL(M, K, N)        \
  MACE_BM_MATMUL_FUNC(M, K, N, Mace);  \
  MACE_BM_MATMUL_FUNC(M, K, N, Eigen);

// Embedding size 384
MACE_BM_MATMUL(7, 384, 384);
MACE_BM_MATMUL(7, 384, 1536);
MACE_BM_MATMUL(7, 1536, 384);

MACE_BM_MATMUL(15, 384, 384);
MACE_BM_MATMUL(15, 384, 1536);
MACE_BM_MATMUL(15, 1536, 384);

MACE_BM_MATMUL(1, 384, 384);
MACE_BM_MATMUL(1, 384, 1536);
MACE_BM_MATMUL(1, 1536, 384);
MACE_BM_MATMUL(1, 384, 44678);

// Embedding size 128
MACE_BM_MATMUL(1, 128, 1536);
MACE_BM_MATMUL(1, 128, 44678);

}  // namespace test
}  // namespace kernels
}  // namespace mace


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <mars/ekf.h>
#include <mars/general_functions/utils.h>
#include <Eigen/Dense>

namespace mars
{
Eigen::MatrixXd Ekf::CalculateCorrection()
{
  // Calculate innovation
  S_ = H_ * P_ * H_.transpose() + R_;
  S_ = Utils::EnforceMatrixSymmetry(S_);

  // Calculate Klamen Gain
  K_ = P_ * H_.transpose() * S_.inverse();

  // Calculate Correction
  Eigen::MatrixXd correction = K_ * res_;

  return correction;
}

Eigen::MatrixXd Ekf::CalculateCovUpdate()
{
  // Calculate ErrorState Covariance
  int64_t state_size = H_.cols();

  Eigen::MatrixXd I_state = Eigen::MatrixXd::Identity(state_size, state_size);

  Eigen::MatrixXd KH = I_state - K_ * H_;
  Eigen::MatrixXd updated_P = KH * P_ * KH.transpose() + K_ * R_ * K_.transpose();

  return updated_P;
}
}


/**
 * \file MT19937.hpp
 * \brief Header for MT19937.
 *
 * This provides an interface to Mersenne Twister random number generator,
 * MT19937.
 * See\n Makoto Matsumoto and Takuji Nishimura,\n Mersenne Twister: A
 * 623-Dimensionally Equidistributed Uniform Pseudo-Random Number Generator,\n
 * ACM TOMACS 8, 3-30 (1998)
 *
 * Interface routines written by Charles
 * Karney <charles@karney.com> and licensed under the LGPL.  For more
 * information, see http://charles.karney.info/random/
 **********************************************************************/

#if !defined(MT19937_H)
#define MT19937_H

#define RCSID_MT19937_H "$Id: MT19937.hpp 6406 2007-05-23 13:29:52Z ckarney $"

#include "RandomLib/RandomSeed.hpp"
#include <limits>

#if HAVE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#endif

/**
 * Which version of seeding and saving to use.  3 = scheme proposed in C++0X
 * random number proposal, version 3 (2006-09) plus append a checksum to data
 * stream (introduced with 2006-11 version).  4 = add leapfrogging (introduced
 * with 2006-12 version).  In this implementation, I/O routines can read
 * versions 3 and 4.
 **********************************************************************/
#define MT19937_VERSION 4

namespace RandomLib {
  /**
   * \brief A generator of random bits.
   *
   * This provides an interface to Mersenne Twister random number generator,
   *
   * MT19937.  See\n Makoto Matsumoto and Takuji Nishimura,\n Mersenne
   * Twister: A 623-Dimensionally Equidistributed Uniform Pseudo-Random Number
   * Generator,\n ACM TOMACS 8, 3-30 (1998)
   *
   * This is adapted from the 32-bit and 64-bit C versions available at
   * http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/emt19937ar.html and
   * http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/emt64.html
   *
   * The template arguments give the type \a UIntType of the "natural" result
   * and the number of random bits \a BitWidth in each result.  Although the
   * two versions of MT19937 produce different sequences, the implementations
   * here are portable across 32-bit and 64-bit architectures.  As expected the
   * 64-bit version of MT19937 runs somewhat faster on 64-bit machines and
   * considerably slower on 32-bit machines.
   *
   * The class supplies the method for advancing the state and generating the
   * random output.  The random results are provided as 32-bit quantities via
   * Ran32(), as 64-bit quantities via Ran64(), and in "natural" units of \a
   * width bits via Ran().  For MT19937, \a width is either 32 or 64.  It
   * inherits from RandomSeed the routines for seed management and for
   * converting the seed into state.
   *
   * It also provides routines for saving and restoring the state of the
   * generator (including its seed), for stepping the generator forwards or
   * backwards, and for testing the equality of two generators.
   *
   * Interface routines written by
   * Charles Karney <charles@karney.com> and licensed under the LGPL.  For
   * more information, see http://charles.karney.info/random/
   **********************************************************************/
  template<typename UIntType, unsigned BitWidth>
  class MT19937 : public RandomSeed {
  public:
    enum {
      /**
       * The number of random bits produced by Ran().
       **********************************************************************/
      width = BitWidth
    };
    /**
     * A type large enough to hold \e width bits.  This is used for the
     * internal state of the generator and the result returned by Ran().
     * (Seeds are specified in terms of the more portable unsigned long.)
     **********************************************************************/
    typedef UIntType result_type;
    /**
     * The minimum result returned by Ran().
     **********************************************************************/
    static const result_type min = 0;
    /**
     * The maximum result returned by Ran() = 2w - 1
     **********************************************************************/
    static const result_type max =
      ~result_type(0) >> std::numeric_limits<result_type>::digits - width;

  protected:
    /**
     * A mask to give the low \e width bits in a result_type.
     **********************************************************************/
    static const result_type RESULT_MASK = max;

  private:
    // Constants
    enum {
      /**
       * The Mersenne prime is 2P - 1
       **********************************************************************/
      P = 19937,
      /**
       * The long lag for MT19937
       **********************************************************************/
      N = (P + width - 1)/width,
      /**
       * The short lag for MT19937
       **********************************************************************/
      M = width == 32 ? 397 : 156,
      /**
       * The number of ignored bits in the first word of the state
       **********************************************************************/
      R = N * width - P
    };
    /**
     * "MTrn" or "MTsn" a signature for Save
     **********************************************************************/
    static const u32 VERSIONID =
      u32(width == 32 ? 0x4d547230UL : 0x4d547330UL) + MT19937_VERSION;
    /**
     * Magic matrix for MT19937
     **********************************************************************/
    static const result_type MATRIX_A =
      result_type(width == 32 ? 0x9908b0dfULL : 0xb5026f5aa96619e9ULL);
    /**
     * Mask for top \a width - \a R bits of a word
     **********************************************************************/
    static const result_type UPPER_MASK = RESULT_MASK << R & RESULT_MASK;
    /**
     * Mask for low \a R bits of a width-bit word
     **********************************************************************/
    static const result_type LOWER_MASK = ~UPPER_MASK & RESULT_MASK;
    /**
     * Marker for uninitialized object
     **********************************************************************/
    static const unsigned UNINIT = 0xffffffffU;

    /**
     * The state vector
     **********************************************************************/
    result_type _state[N];
    /**
     * The index for the next random value
     **********************************************************************/
    unsigned _ptr;
    /**
     * How many times has NextBatch() been called
     **********************************************************************/
    long long _rounds;
    /**
     * Stride for leapfrogging
     **********************************************************************/
    unsigned _stride;

  public:

    /** \name Constructors which set the seed
     **********************************************************************/
    ///@{
    /**
     * Initialize from a vector.  Only the low \e 32 bits of each element are
     * used.
     **********************************************************************/
    template<typename IntType> explicit MT19937(const std::vector<IntType>& v)
      throw(std::bad_alloc) { Reseed(v); }
    /**
     * Initialize from a pair of iterators setting seed to [\a a, \a b).  The
     * iterator must produce results which can be converted into seed_type.
     * Only the low \e 32 bits of each element are used.
     **********************************************************************/
    template<typename InputIterator> MT19937(InputIterator a, InputIterator b)
    { Reseed(a, b); }
    /**
     * Initialize with seed [\a n].  Only the low \e width bits of \a n are
     * used.
     **********************************************************************/
    explicit MT19937(seed_type n) throw(std::bad_alloc)
    { Reseed(n); }
    /**
     * Initialize with seed [SeedWord()]
     **********************************************************************/
    MT19937() throw(std::bad_alloc) { Reseed(); }
    /**
     * Initialize from a string.  See Reseed(const std::string& s)
     **********************************************************************/
    explicit MT19937(const std::string& s) throw(std::bad_alloc)
    { Reseed(s); }
    ///@}

    /** \name Functions for returning random data
     **********************************************************************/
    ///@{
    /**
     * Return \e width bits of randomness.  Result is in [0,
     * 2w)
     **********************************************************************/
    result_type operator()() throw() { return Ran(); }
    ///@}

    /** \name Comparing Random objects
     **********************************************************************/
    ///@{
    /**
     * Test equality of two Random objects.  This test that the seeds match and
     * that they have produced the same number of random numbers.
     **********************************************************************/
    bool operator==(const MT19937& r) const throw()
    // Ensure that the two Random objects behave the same way.  Note however
    // that the internal states may still be different, e.g., the following all
    // result in Random objects which are == (with Count() == 0) but which all
    // have different internal states:
    //
    // Random r(0);                       _ptr == UNINIT
    // r.StepCount( 1); r.StepCount(-1);  _ptr == 0, _rounds ==  0
    // r.StepCount(-1); r.StepCount( 1);  _ptr == N, _rounds == -1
    { return Count() == r.Count() && _seed == r._seed &&
	_stride == r._stride; }
    /**
     * Test inequality of two Random objects.  See Random::operator==
     **********************************************************************/
    bool operator!=(const MT19937& r) const throw()
    { return !operator==(r); }
    ///@}

    /** \name Writing to and reading from a stream
     **********************************************************************/
    ///@{
    /**
     * Save the state of the Random object to an output stream.  Format is a
     * sequence of unsigned 32-bit integers written either in decimal (\a bin
     * false, text format) or in network order with most significant byte first
     * (\a bin true, binary format).  Data consists of:
     *
     *  - VERSIONID (1 word)
     *  - _seed.size() (1 word)
     *  - _seed data (_seed.size() words)
     *  - _ptr (1 word)
     *  - _stride (1 word)
     *  - if _ptr != UNINIT, _rounds (2 words)
     *  - if _ptr != UNINIT, _state (N words or 2 N words)
     *  - checksum
     *
     * Shortest possible saved result consists of 5 words:
     *  - VERSIONID = "MTrn"
     *  - _seed.size() = 0
     *  - _ptr = UNINIT
     *  - _stride = 1
     *  - checksum
     *
     * This corresponds to Seed() = [] and Count() = 0.
     **********************************************************************/
    void Save(std::ostream& os, bool bin = true) const
      throw(std::ios::failure);
    /**
     * Restore the state of the Random object from an input stream.  If \a bin,
     * read in binary, else use text format.  See documentation of Random::Save
     * for the format.  Include error checking on date to make sure the input
     * has not been corrupted.  If an error occurs while reading, the Random
     * object is unchanged.
     **********************************************************************/
    void Load(std::istream& is, bool bin = true)
      throw(std::ios::failure, std::out_of_range, std::bad_alloc) {
      // Read state into temporary so as not to change object on error.
      MT19937 t(is, bin);
      _seed.reserve(t._seed.size());
      *this = t;
    }
    ///@}

    /** \name Examining and advancing the Random generator
     **********************************************************************/
    ///@{
    /**
     * Return the number of random numbers used.  This needs to return a long
     * long result since it can reasonably exceed 231.  (On a 1GHz
     * machine, it takes about a minute to produce 232 random
     * numbers.)  More precisely this is the (zero-based) index of the next
     * random number to be produced.  (This distinction is important when
     * leapfrogging is in effect.)
     **********************************************************************/
    long long Count() const throw()
    { return _ptr == UNINIT ? 0 : _rounds * N + _ptr; }
    /**
     * Step the generator forwards of backwarks so that the value returned
     * by Count() is \a n
     **********************************************************************/
    void SetCount(long long n) throw() { StepCount(n - Count()); }
    /**
     * Step the generator forward \a n steps.  \a n can be negative.
     **********************************************************************/
    void StepCount(long long n) throw();
    /**
     * Resets the sequence.  Equivalent to SetCount(0), but works by
     * reinitializing the Random object from its seed, rather than by stepping
     * the sequence backwards.  In addition, this undoes leapfrogging.
     **********************************************************************/
    void Reset() throw() { _ptr = UNINIT; _stride = 1; }
    ///@}

    /** \name Leapfrogging
     **********************************************************************/
    ///@{
    /**
     * Set leapfrogging stride to a positive number \a n and increment Count()
     * by \a k < \a n.  If the current Count() is \a i, then normally the next
     * 3 random numbers would have indices \a i, \a i + 1, \a i + 2, and the
     * new Count() is \a i + 2.  However, after SetStride(\a n, \a k) the next
     * 3 random numbers have indices \a i + \a k, \a i + \a k + \a n, \a i + \a
     * k + 2\a n, and the new Count() is \a i + \a k + 3\a n.  With
     * leapfrogging in effect, the time to produce raw random numbers is
     * roughly proportional to 1 + (\a n - 1)/2.  Reseed(...) and Reset() both
     * reset the stride back to 1.  See \ref leapfrog "Leapfrogging" for a
     * description of how to use this facility.
     **********************************************************************/
    void SetStride(unsigned n = 1, unsigned k = 0)
      throw(std::invalid_argument) {
      // Limit stride to UNINIT/2.  This catches negative numbers that have
      // been cast into unsigned.  In reality the stride should be no more than
      // 10-100.
      if (n == 0 || n > UNINIT/2)
	throw std::invalid_argument("MT19937: Invalid stride");
      if (k >= n)
	throw std::invalid_argument("MT19937: Invalid index");
      _stride = n;
      StepCount(k);
    }
    /**
     * Return leapfrogging stride.
     **********************************************************************/
    unsigned GetStride() const throw() { return _stride; }
    ///@}

    /**
     * Return string description of this generator
     **********************************************************************/
    static std::string Name() {
      std::ostringstream s;
      s << "RandomLib::MT19937 " << width << "-bit "
	<< "Version " << MT19937_VERSION;
      return s.str();
    }

    /**
     * Tests basic engine.  Throws out_of_range errors on bad results.
     **********************************************************************/
    static void SelfTest() {
      MT19937 g(std::vector<seed_type>(0));
      g.SetCount(10000-1);
      if (g() !=
	  result_type(width == 32 ? 4123659995ULL : 9981545732273789042ULL))
	throw std::out_of_range("MT19937: Incorrect result with seed " +
				g.SeedString());
      seed_type s[] = {1, 2, 3, 4};
      g.Reseed(s, s+4);
      g.SetCount(-10000);
      std::string save;
      {
	std::ostringstream stream;
	stream << g << std::endl;
	save = stream.str();
      }
      g.Reset();
      {
	std::istringstream stream(save);
	stream >> g;
      }
      g.SetCount(10000);
      {
	std::ostringstream stream;
	g.Save(stream, true);
	save = stream.str();
      }
      {
	std::istringstream stream(save);
	MT19937 h(std::vector<seed_type>(0));
	h.Load(stream, true);
	h.SetCount(1000000-1);
	if (h() !=
	    result_type(width == 32 ? 4244229310ULL : 16856309709668453709ULL))
	throw std::out_of_range("MT19937: Incorrect result with seed " +
				h.SeedString());
	g.SetCount(1000000);
	if (h != g)
	throw std::out_of_range("MT19937: Comparison failure");
      }
    }

  protected:
    /**
     * Return \e width bits of randomness.  This is the natural unit of random
     * data produced by MT19937.
     **********************************************************************/
    result_type Ran() throw();
    /**
     * Return 32 bits of randomness.
     **********************************************************************/
    u32 Ran32() throw();
    /**
     * Return 64 bits of randomness.
     **********************************************************************/
    u64 Ran64() throw();

  private:
    /**
     * Compute initial state from seed
     **********************************************************************/
    void Init() throw();
    /**
     * Advance state by \e N steps
     **********************************************************************/
    void NextBatch(unsigned long long count) throw();
    /**
     * Back up state by \e N steps
     **********************************************************************/
    void PreviousBatch(unsigned long long count) throw();
    /**
     * The interface to NextBatch used by Ran().
     **********************************************************************/
    void Next() throw();

    /**
     * Consistency check on state; mainly used for validating I/O.  This throws
     * an out-of-range error if the state is bad.  It also computes a simple
     * checksum to ensure the integrity of restored data.
     **********************************************************************/
    u32 Check(u32 version) const throw(std::out_of_range);

    /**
     * Read from an input stream.  Potentially corrupts object.  This is used
     * by Load so that it can avoid corrupting its state on bad input.
     **********************************************************************/
    explicit MT19937(std::istream& is, bool bin)
      throw(std::ios::failure, std::out_of_range, std::bad_alloc);

#if HAVE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    /**
     * Save to a boost archive.  Boost versioning isn't very robust.  (It
     * allows a RandomGenerator32 to be read back in as a RandomGenerator64.
     * It doesn't interact well with templates.)  So we do our own versioning
     * and supplement this with a checksum.
     **********************************************************************/
    template<class Archive> void save(Archive& ar, const unsigned int) const {
      u32 _version = VERSIONID,
	_checksum = Check(_version);
      ar & boost::serialization::make_nvp("version" , _version )
	&  boost::serialization::make_nvp("seed"    , _seed    )
	&  boost::serialization::make_nvp("ptr"     , _ptr     )
	&  boost::serialization::make_nvp("stride"  , _stride  );
      if (_ptr != UNINIT)
	ar & boost::serialization::make_nvp("rounds", _rounds  )
	  &  boost::serialization::make_nvp("state" , _state   );
      ar & boost::serialization::make_nvp("checksum", _checksum);
    }
    /**
     * Load from a boost archive.  Do this safely so that the current object is
     * not corrupted if the archive is bogus.
     **********************************************************************/
    template<class Archive> void load(Archive& ar, const unsigned int) {
      u32 _version, _checksum;
      ar & boost::serialization::make_nvp("version" , _version );
      MT19937 t(std::vector<seed_type>(0));
      ar & boost::serialization::make_nvp("seed"    , t._seed  )
	&  boost::serialization::make_nvp("ptr"     , t._ptr   );
      if (_version == VERSIONID - 1) {
	if (t._ptr == N + 1)
	  t._ptr = UNINIT;
	t._stride = 1;
      } else
	ar &  boost::serialization::make_nvp("stride"  , t._stride);
      if (t._ptr != UNINIT)
	ar & boost::serialization::make_nvp("rounds", t._rounds)
	  &  boost::serialization::make_nvp("state" , t._state );
      ar & boost::serialization::make_nvp("checksum", _checksum);
      if (t.Check(_version) != _checksum)
	throw std::out_of_range("MT19937: Checksum failure");
      _seed.reserve(t._seed.size());
      *this = t;
    }
    /**
     * Glue the boost save and load functionality together -- a bit of boost
     * magic.
     **********************************************************************/
    template<class Archive>
    void serialize(Archive &ar, const unsigned int file_version)
    { boost::serialization::split_member(ar, *this, file_version); }
#endif
  };

  template<>
  inline RandomSeed::u32 MT19937<RandomSeed::u32, 32>::Ran() throw() {
    // On exit we have _stride <= _ptr < N + _stride.
    if (_ptr >= N)
      Next();
    result_type y = _state[_ptr];
    _ptr += _stride;

    // Specific tempering instantiation for width = 32 given in
    // http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/emt19937ar.html
    y ^= y >> 11;
    y ^= y <<  7 & result_type(0x9d2c5680UL);
    y ^= y << 15 & result_type(0xefc60000UL);
    y ^= y >> 18;

    return y;
  }

  template<>
  inline RandomSeed::u32 MT19937<RandomSeed::u32, 32>::Ran32() throw()
  { return Ran(); }

  template<>
  inline RandomSeed::u64 MT19937<RandomSeed::u32, 32>::Ran64() throw()
  { const u64 x = Ran(); return x << width | static_cast<u64>(Ran()); }

  template<>
  inline RandomSeed::u64 MT19937<RandomSeed::u64, 64>::Ran() throw() {
    if (_ptr >= N)
      Next();
    result_type y = _state[_ptr];
    _ptr += _stride;

    // Specific tempering instantiation for width = 64 given in
    // http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/emt64.html
    y ^= y >> 29 & result_type(0x5555555555555555ULL);
    y ^= y << 17 & result_type(0x71d67fffeda60000ULL);
    y ^= y << 37 & result_type(0xfff7eee000000000ULL);
    y ^= y >> 43;

    return y;
  }

  template<>
  inline RandomSeed::u32 MT19937<RandomSeed::u64, 64>::Ran32() throw()
  { return static_cast<u32>(Ran()) & U32_MASK; }

  template<>
  inline RandomSeed::u64 MT19937<RandomSeed::u64, 64>::Ran64() throw()
  { return Ran(); }
} // namespace RandomLib
#endif	// MT19937_H


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 combinatorial toolbox - is_prime/expr Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of combinatorial components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 07/03/2011
///
#include <nt2/toolbox/combinatorial/include/functions/is_prime.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>

#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/toolbox/constant/constant.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/table.hpp>

NT2_TEST_CASE_TPL ( is_prime_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::is_prime;
  using nt2::tag::is_prime_;
  typedef typename nt2::meta::as_logical<T>::type bT;
  nt2::table<T> a = nt2::_(T(1), T(30));
  int aa[] = {false, true, true, false, true, false, true, false, false, false,
               true, false, true, false, false, false, true, false, true, false,
               false, false, true, false, false, false, false, false, true, false};
  nt2::table<bT> ba(nt2::of_size(1, 30));

  for(int i=0; i < 30; i++) ba(i+1) = nt2::is_nez(aa[i]);
  NT2_DISPLAY( is_prime(a));
  nt2::table<bT> p = is_prime(a);
  NT2_TEST_EQUAL(p, ba);

} // end of test for floating_

NT2_TEST_CASE_TPL ( is_prime_integer__1_0,  NT2_INTEGRAL_TYPES)
{

  using nt2::is_prime;
  using nt2::tag::is_prime_;
  NT2_TEST(!is_prime(nt2::Eight<T>()));
  NT2_TEST(is_prime(nt2::Seven<T>()));
  NT2_TEST(is_prime(nt2::Two<T>()));
  NT2_TEST(!is_prime(nt2::One<T>()));
 } // end of test for integer_


#include <metaSMT/frontend/QF_UF.hpp>
#include <metaSMT/frontend/QF_BV.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/dynamic_bitset.hpp>
#include <string>

using namespace metaSMT;
using namespace metaSMT::logic;
using namespace metaSMT::logic::QF_UF;
using namespace metaSMT::logic::QF_BV;
namespace proto = boost::proto;
using boost::dynamic_bitset;

BOOST_FIXTURE_TEST_SUITE(QF_UF, Solver_Fixture )

BOOST_AUTO_TEST_CASE( equal_bool ) {
  using namespace type;

  unsigned const w = 8;
  Uninterpreted_Function f = declare_function(Boolean())(BitVector(w));
  bitvector x = new_bitvector(w);

  assertion(ctx, equal(f(x), f(x)) );
  BOOST_REQUIRE( solve(ctx) );

  assertion(ctx, nequal(f(x), f(x)) );
  BOOST_REQUIRE( !solve(ctx) );
}

BOOST_AUTO_TEST_CASE( equal_bitvector ) {
  using namespace type;
  unsigned const result_width = 4;
  unsigned const param_width = 8;
  Uninterpreted_Function f = declare_function(BitVector(result_width))(BitVector(param_width));
  bitvector x = new_bitvector(param_width);

  assertion(ctx, equal(f(x), f(x)) );
  BOOST_REQUIRE( solve(ctx) );

  assertion(ctx, nequal(f(x), f(x)) );
  BOOST_REQUIRE( !solve(ctx) );
}

BOOST_AUTO_TEST_CASE( functional_consistency ) {
  using namespace type;

  unsigned const w = 8;
  Uninterpreted_Function f = declare_function(Boolean())(BitVector(w));
  bitvector x = new_bitvector(w);
  bitvector y = new_bitvector(w);

  // functional consistency:
  //   ( x == y ) --> ( f(x) == f(y) )
  assertion(ctx, equal(x, y) );

  assertion(ctx, equal(f(x), f(y)) );
  BOOST_REQUIRE( solve(ctx) );
}

BOOST_AUTO_TEST_CASE( two_arguments ) {
  using namespace type;
  unsigned const w = 8;

  Uninterpreted_Function f = declare_function(Boolean())(BitVector(w))(BitVector(w));
  bitvector x = new_bitvector(w);

  assertion(ctx, equal(f(x,x), f(x,x)));
  BOOST_REQUIRE( solve(ctx) );
}

BOOST_AUTO_TEST_CASE( three_arguments ) {
  using namespace type;
  unsigned const w = 8;

  Uninterpreted_Function f = declare_function(Boolean())(BitVector(w))(BitVector(w))(BitVector(w));
  bitvector x = new_bitvector(w);

  assertion(ctx, equal(f(x,x,x), f(x,x,x)));
  BOOST_REQUIRE( solve(ctx) );
}

BOOST_AUTO_TEST_CASE( variable_equality ) {
  using namespace type;

  unsigned const w = 8;
  Uninterpreted_Function f = declare_function(Boolean())(BitVector(w));
  Uninterpreted_Function g = declare_function(Boolean())(BitVector(w));

  bool cmp = (f == f);
  BOOST_CHECK( cmp );

  cmp = (g == f);
  BOOST_CHECK( !cmp );

  cmp = (f == g);
  BOOST_CHECK( !cmp );
}

BOOST_AUTO_TEST_SUITE_END() // QF_UF

//  vim: ft=cpp:ts=2:sw=2:expandtab


/*
 * @Description: IMU data
 * @Author: Ge Yao
 * @Date: 2020-11-10 14:25:03
 */
#ifndef IMU_INTEGRATION_IMU_DATA_HPP_
#define IMU_INTEGRATION_IMU_DATA_HPP_

#include <Eigen/Dense>
#include <Eigen/Core>

namespace imu_integration {

struct IMUData {
    double time = 0.0;
    Eigen::Vector3d linear_acceleration = Eigen::Vector3d::Zero();
    Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero();
};

} // namespace imu_integration

#endif


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file sigma_point_quadrature_test.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>
#include "../typecast.hpp"

#include <Eigen/Dense>

#include <random>

#include <fl/util/profiling.hpp>
#include <fl/util/math.hpp>
#include <fl/distribution/gaussian.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/transform/unscented_transform.hpp>
#include <fl/filter/gaussian/transform/monte_carlo_transform.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

template <typename TestType>
class SigmaPointQuadratureTests
    : public testing::Test
{
public:
    typedef typename TestType::Parameter Configuration;

    enum: signed int
    {
        DimA = Configuration::DimA,
        DimB = Configuration::DimB,

        SizeA = fl::TestSize<DimA, TestType>::Value,
        SizeB = fl::TestSize<DimB, TestType>::Value
    };

    typedef Eigen::Matrix<fl::Real, SizeA, 1> VariateA;
    typedef Eigen::Matrix<fl::Real, SizeB, 1> VariateB;

    typedef fl::Gaussian<VariateA> GaussianA;
    typedef fl::Gaussian<VariateB> GaussianB;

    typedef Eigen::Matrix<fl::Real, SizeA, SizeA> MatrixAA;
    typedef Eigen::Matrix<fl::Real, SizeB, SizeA> MatrixAB;

    typedef typename Configuration::TransformSelection::Transform Transform;
    typedef fl::SigmaPointQuadrature<Transform> Quadrature;

    SigmaPointQuadratureTests()
        : F(MatrixAA::Random(DimA, DimA)),
          H(MatrixAB::Random(DimB, DimA)),
          p_A(DimA),
          p_B(DimB),
          quadrature(Transform()),
          eps(Configuration::TransformSelection::epsilon)
    {
        // create a random source Gaussians
        p_A.mean(VariateA::Random(DimA));
        p_A.covariance(p_A.covariance() * fl::Real(std::rand()) / RAND_MAX);

        p_B.mean(VariateB::Random(DimB));
        p_B.covariance(p_B.covariance() * fl::Real(std::rand()) / RAND_MAX);
    }

    template <typename Var> Var f(const Var& x)
    {
        return F * x;
    }

    template <typename VarA, typename VarB>
    VarB f(const VarA& x, const VarB& w)
    {
        return H * x + w;
    }

    void integrate_fx_px()
    {
        using namespace fl;

        // compute the expected integration result analytically
        auto expect_gaussian = GaussianA(DimA);
        expect_gaussian.mean(f(p_A.mean()));
        expect_gaussian.covariance(F * p_A.covariance() * F.transpose());

        // create the gaussian which will contail the integration results
        auto result_gaussian = GaussianA(DimA);

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x) { return f(x); };

        // define the expected covariance lambda function
        auto cov_f = [&] (const VariateA& x)
        {
            return ((f(x) - result_gaussian.mean()) *
                    (f(x) - result_gaussian.mean()).transpose()).eval();
        };

        // integrate mean and covariance
        result_gaussian.mean(quadrature.integrate(mean_f, p_A));
        result_gaussian.covariance(quadrature.integrate(cov_f, p_A));

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));
    }

    void integrate_fxy_pxy()
    {
        using namespace fl;

        auto expect_gaussian = GaussianB(DimB);
        auto result_gaussian = GaussianB(DimB);

        expect_gaussian.mean(f(p_A.mean(), p_B.mean()));
        expect_gaussian.covariance(
            H * p_A.covariance() * H.transpose() + p_B.covariance());

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x, const VariateB& y)
        {
            return f(x, y);
        };

        // define the expected covariance lambda function
        auto cov_f = [&] (const VariateA& x, const VariateB& y)
        {
            return ((f(x, y) - result_gaussian.mean()) *
                    (f(x, y) - result_gaussian.mean()).transpose()).eval();
        };

        // integrate mean and covariance
        result_gaussian.mean(quadrature.integrate(mean_f, p_A, p_B));
        result_gaussian.covariance(quadrature.integrate(cov_f, p_A, p_B));

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));
    }

    void propergate_gaussian_Z()
    {
        using namespace fl;

        auto result_gaussian = GaussianA(DimA);
        auto expect_gaussian = GaussianA(DimA);
        expect_gaussian.mean(f(p_A.mean()));
        expect_gaussian.covariance(F * p_A.covariance() * F.transpose());

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x) { return f(x); };

        enum { SetSize = Quadrature::template size<VariateA>() };

        auto Z = PointSet<decltype(mean_f(VariateA())), SetSize>();

        quadrature.propergate_gaussian(mean_f, p_A, Z);

        auto mean = Z.center();
        auto Z_c = Z.points();
        auto W = Z.covariance_weights_vector();

        // integrate mean and covariance
        result_gaussian.mean(mean);
        result_gaussian.covariance(Z_c * W.asDiagonal() * Z_c.transpose());

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));
    }

    void propergate_gaussian_X_Z()
    {
        using namespace fl;

        auto result_gaussian = GaussianA(DimA);
        auto expect_gaussian = GaussianA(DimA);
        expect_gaussian.mean(f(p_A.mean()));
        expect_gaussian.covariance(F * p_A.covariance() * F.transpose());

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x) { return f(x); };

        enum { SetSize = Quadrature::template size<VariateA>() };

        auto Y = PointSet<VariateA, SetSize>();
        auto Z = PointSet<decltype(mean_f(VariateA())), SetSize>();

        // integrate and check moment results
        quadrature.propergate_gaussian(mean_f, p_A, Y, Z);

        auto mean = Z.center();
        auto Z_c = Z.points();
        auto W = Z.covariance_weights_vector();

        // integrate mean and covariance
        result_gaussian.mean(mean);
        result_gaussian.covariance(Z_c * W.asDiagonal() * Z_c.transpose());

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));

        // finally check the Y result whether it actually represents the
        // input Gaussian p_A
        auto temp_gaussian = GaussianA(DimA);

        // center points and get the mean
        auto Y_mean = Y.center();

        // compute point covariacne
        auto Y_c = Y.points();
        auto Y_W = Y.covariance_weights_vector();
        auto Y_cov = Y_c * Y_W.asDiagonal() * Y_c.transpose();

        temp_gaussian.mean(Y_mean);
        temp_gaussian.covariance(Y_cov);
        EXPECT_TRUE(temp_gaussian.is_approx(p_A, eps, true));
    }

    void propergate_gaussian_pxy_Z()
    {
        using namespace fl;

        auto expect_gaussian = GaussianB(DimB);
        auto result_gaussian = GaussianB(DimB);

        expect_gaussian.mean(f(p_A.mean(), p_B.mean()));
        expect_gaussian.covariance(
            H * p_A.covariance() * H.transpose() + p_B.covariance());

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x, const VariateB& y)
        {
            return f(x, y);
        };

        enum { SetSize = Quadrature::template size<VariateA, VariateB>() };

        auto Z = PointSet<decltype(mean_f(VariateA(), VariateB())), SetSize>();

        quadrature.propergate_gaussian(mean_f, p_A, p_B, Z);

        auto mean = Z.center();
        auto Z_c = Z.points();
        auto W = Z.covariance_weights_vector();

        // integrate mean and covariance
        result_gaussian.mean(mean);
        result_gaussian.covariance(Z_c * W.asDiagonal() * Z_c.transpose());

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));
    }

    void propergate_gaussian_pxy_X_Y_Z()
    {
        using namespace fl;

        auto expect_gaussian = GaussianB(DimB);
        auto result_gaussian = GaussianB(DimB);

        expect_gaussian.mean(f(p_A.mean(), p_B.mean()));
        expect_gaussian.covariance(
            H * p_A.covariance() * H.transpose() + p_B.covariance());

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x, const VariateB& y)
        {
            return f(x, y);
        };

        enum { SetSize = Quadrature::template size<VariateA, VariateB>() };

        auto X = PointSet<VariateA, SetSize>();
        auto Y = PointSet<VariateB, SetSize>();
        auto Z = PointSet<decltype(mean_f(VariateA(), VariateB())), SetSize>();

        quadrature.propergate_gaussian(mean_f, p_A, p_B, X, Y, Z);

        auto mean = Z.center();
        auto Z_c = Z.points();
        auto W = Z.covariance_weights_vector();

        // integrate mean and covariance
        result_gaussian.mean(mean);
        result_gaussian.covariance(Z_c * W.asDiagonal() * Z_c.transpose());

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));

        // finally check the X and Y result whether they actually represents the
        // input Gaussians p_A and p_B, respectively
        auto temp_gaussian_A = GaussianA(DimA);
        auto temp_gaussian_B = GaussianB(DimB);

        // center points and get the mean
        auto X_mean = X.center();
        auto Y_mean = Y.center();

        // compute point covariacne
        auto X_c = X.points();
        auto Y_c = Y.points();

        auto X_W = X.covariance_weights_vector();
        auto Y_W = Y.covariance_weights_vector();

        auto X_cov = X_c * X_W.asDiagonal() * X_c.transpose();
        auto Y_cov = Y_c * Y_W.asDiagonal() * Y_c.transpose();

        temp_gaussian_A.mean(X_mean);
        temp_gaussian_A.covariance(X_cov);
        EXPECT_TRUE(temp_gaussian_A.is_approx(p_A, eps, true));

        temp_gaussian_B.mean(Y_mean);
        temp_gaussian_B.covariance(Y_cov);
        EXPECT_TRUE(temp_gaussian_B.is_approx(p_B, eps, true));
    }

    void integrate_moments_fx_px()
    {
        using namespace fl;

        // compute the expected integration result analytically
        auto expect_gaussian = GaussianA(DimA);
        expect_gaussian.mean(f(p_A.mean()));
        expect_gaussian.covariance(F * p_A.covariance() * F.transpose());

        // create the gaussian which will contail the integration results
        auto result_gaussian = GaussianA(DimA);

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x) { return f(x); };

        // integrate mean and covariance
        auto mean = typename FirstMomentOf<VariateA>::Type();
        auto cov = typename SecondMomentOf<VariateA>::Type();
        quadrature.integrate_moments(mean_f, p_A, mean, cov);

        result_gaussian.mean(mean);
        result_gaussian.covariance(cov);

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));
    }

    void integrate_moments_fxy_pxy()
    {
        using namespace fl;

        // compute the expected integration result analytically
        auto expect_gaussian = GaussianB(DimB);

        expect_gaussian.mean(f(p_A.mean(), p_B.mean()));
        expect_gaussian.covariance(
            H * p_A.covariance() * H.transpose() + p_B.covariance());

        // define the expected mean lambda function
        // here we use a simple identity function
        auto mean_f = [&] (const VariateA& x, const VariateB& y)
        {
            return f(x, y);
        };

        // integrate mean and covariance
        auto mean = typename FirstMomentOf<VariateB>::Type();
        auto cov = typename SecondMomentOf<VariateB>::Type();
        quadrature.integrate_moments(mean_f, p_A, p_B, mean, cov);

        // create the gaussian which will contail the integration results
        auto result_gaussian = GaussianB(DimB);

        result_gaussian.mean(mean);
        result_gaussian.covariance(cov);

        EXPECT_TRUE(result_gaussian.is_approx(expect_gaussian, eps, true));
    }

protected:
    /* parameter of the linear function f(x) = A*x */
    MatrixAA F;
    MatrixAB H;

    /* source Gaussian distributions used within the integrals */
    GaussianA p_A;
    GaussianB p_B;

    /* our integrator */
    Quadrature quadrature;

    fl::Real eps;
};

TYPED_TEST_CASE_P(SigmaPointQuadratureTests);

TYPED_TEST_P(SigmaPointQuadratureTests, integrate_fx_px)
{
    TestFixture::integrate_fx_px();
}

TYPED_TEST_P(SigmaPointQuadratureTests, integrate_fxy_pxy)
{
    TestFixture::integrate_fxy_pxy();
}

TYPED_TEST_P(SigmaPointQuadratureTests, propergate_gaussian_Z)
{
    TestFixture::propergate_gaussian_Z();
}

TYPED_TEST_P(SigmaPointQuadratureTests, propergate_gaussian_X_Z)
{
    TestFixture::propergate_gaussian_X_Z();
}

TYPED_TEST_P(SigmaPointQuadratureTests, propergate_gaussian_pxy_Z)
{
    TestFixture::propergate_gaussian_pxy_Z();
}

TYPED_TEST_P(SigmaPointQuadratureTests, propergate_gaussian_pxy_X_Y_Z)
{
    TestFixture::propergate_gaussian_pxy_X_Y_Z();
}

TYPED_TEST_P(SigmaPointQuadratureTests, integrate_moments_fx_px)
{
    TestFixture::integrate_moments_fx_px();
}

TYPED_TEST_P(SigmaPointQuadratureTests, integrate_moments_fxy_pxy)
{
    TestFixture::integrate_moments_fxy_pxy();
}

REGISTER_TYPED_TEST_CASE_P(SigmaPointQuadratureTests,
                           integrate_fx_px,
                           integrate_fxy_pxy,
                           propergate_gaussian_Z,
                           propergate_gaussian_X_Z,
                           propergate_gaussian_pxy_Z,
                           propergate_gaussian_pxy_X_Y_Z,
                           integrate_moments_fx_px,
                           integrate_moments_fxy_pxy);

namespace internal
{

// Transform configuration selection helper
template <typename T> struct TransformSelection;

// TransformSelection for MonteCarlo integration
template <typename PSP>
struct TransformSelection<fl::MonteCarloTransform<PSP>>
{
    static constexpr fl::Real epsilon = fl::Real(0.5);
    typedef fl::MonteCarloTransform<PSP> Transform;
};

// TransformSelection for deterministic Unscented integration
template <> struct TransformSelection<fl::UnscentedTransform>
{
    static constexpr fl::Real epsilon = fl::Real(1.e-9);
    typedef fl::UnscentedTransform Transform;
};

}

template <int DimensionA, int DimensionB, typename Transform>
struct TestConfiguration
{
    enum : signed int
    {
        DimA = DimensionA,
        DimB = DimensionB
    };

    typedef internal::TransformSelection<Transform> TransformSelection;
};


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

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <boost/assert.hpp>
#include <benchmark/benchmark.h>
#include <vector>
#include <algorithm>
#include <random>
#include <execution>
#include <iostream>
#include <iterator>

template<class T>
std::vector<T> generate_random_vector(std::size_t size, std::size_t seed)
{
    if (seed == 0)
    {
        std::random_device rd;
        seed = rd();
    }
    std::vector<T> v(size);

    std::mt19937 gen(seed);

    if constexpr (std::is_floating_point<T>::value)
    {
        std::normal_distribution<T> dis(0, 1);
        for (size_t i = 0; i < v.size(); ++i)
        {
            v[i] = dis(gen);
        }
        return v;
    }
    else if constexpr (std::is_integral<T>::value)
    {
        // Rescaling by larger than 2 is UB!
        std::uniform_int_distribution<T> dis(std::numeric_limits<T>::lowest()/2, (std::numeric_limits<T>::max)()/2);
        for (size_t i = 0; i < v.size(); ++i)
        {
            v[i] = dis(gen);
        }
        return v;
    }
    else if constexpr (boost::is_complex<T>::value)
    {
        std::normal_distribution<typename T::value_type> dis(0, 1);
        for (size_t i = 0; i < v.size(); ++i)
        {
            v[i] = {dis(gen), dis(gen)};
        }
        return v;
    }
    else if constexpr (boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_complex)
    {
        std::normal_distribution<long double> dis(0, 1);
        for (size_t i = 0; i < v.size(); ++i)
        {
            v[i] = {dis(gen), dis(gen)};
        }
        return v;
    }
    else if constexpr (boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_floating_point)
    {
        std::normal_distribution<long double> dis(0, 1);
        for (size_t i = 0; i < v.size(); ++i)
        {
            v[i] = dis(gen);
        }
        return v;
    }
    else
    {
        BOOST_ASSERT_MSG(false, "Could not identify type for random vector generation.");
        return v;
    }
}

template<typename T>
void mean(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::mean(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_mean(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::mean(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void variance(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::variance(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_variance(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::variance(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void skewness(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::skewness(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_skewness(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::skewness(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void first_four_moments(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::first_four_moments(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_first_four_moments(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::first_four_moments(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void kurtosis(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::kurtosis(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_kurtosis(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::kurtosis(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void median(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::median(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_median(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::median(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void median_absolute_deviation(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::median_absolute_deviation(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_median_absolute_deviation(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::median_absolute_deviation(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void gini_coefficient(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::gini_coefficient(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_gini_coefficient(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::gini_coefficient(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void interquartile_range(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::interquartile_range(std::execution::seq, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_interquartile_range(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::interquartile_range(std::execution::par, test_set));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void mode(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);
    std::vector<T> modes;

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::mode(std::execution::seq, test_set, std::back_inserter(modes)));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void parallel_mode(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> test_set = generate_random_vector<T>(size, seed);
    std::vector<T> modes;

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::mode(std::execution::par, test_set, std::back_inserter(modes)));
    }
    state.SetComplexityN(state.range(0));
}

// Mean
BENCHMARK_TEMPLATE(mean, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_mean, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(mean, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_mean, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Variance
BENCHMARK_TEMPLATE(variance, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_variance, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(variance, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_variance, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Skewness
BENCHMARK_TEMPLATE(skewness, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_skewness, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(skewness, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_skewness, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// First four moments
BENCHMARK_TEMPLATE(first_four_moments, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_first_four_moments, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(first_four_moments, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_first_four_moments, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Kurtosis
BENCHMARK_TEMPLATE(kurtosis, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_kurtosis, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(kurtosis, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_kurtosis, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Median
BENCHMARK_TEMPLATE(median, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_median, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(median, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_median, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Median absolute deviation
BENCHMARK_TEMPLATE(median_absolute_deviation, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_median_absolute_deviation, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(median_absolute_deviation, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_median_absolute_deviation, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Gini Coefficient
BENCHMARK_TEMPLATE(gini_coefficient, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_gini_coefficient, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(gini_coefficient, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_gini_coefficient, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Interquartile Range - Only floating point values implemented
BENCHMARK_TEMPLATE(interquartile_range, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_interquartile_range, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

// Mode
BENCHMARK_TEMPLATE(mode, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_mode, int)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(mode, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(parallel_mode, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

BENCHMARK_MAIN();


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "mitkConnectomicsStatisticsCalculator.h"
#include "mitkConnectomicsNetworkConverter.h"

#include <numeric>

#include <boost/graph/clustering_coefficient.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4172)
#endif
#include <boost/graph/connected_components.hpp>
#include <boost/graph/betweenness_centrality.hpp>

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#include <boost/graph/visitors.hpp>

#include "vnl/algo/vnl_symmetric_eigensystem.h"

template<typename GraphType>
class all_pairs_shortest_recorder : public boost::default_bfs_visitor
{
public:
  typedef typename boost::graph_traits<GraphType>::vertex_descriptor VertexType;
  typedef typename boost::graph_traits<GraphType>::edge_descriptor EdgeType;

  all_pairs_shortest_recorder(int* dist, int &max, unsigned int &size)
  {
    d = dist;
    ecc = &max;
    component_size = &size;
  }

  void tree_edge(EdgeType edge, const GraphType& graph) {
    VertexType u = boost::source(edge, graph);
    VertexType v = boost::target(edge, graph);
    d[v] = d[u] + 1;
    *ecc = d[v];
    *component_size = *component_size + 1;
  }
private:
  int* d;
  int* ecc;
  unsigned int* component_size;
};

mitk::ConnectomicsStatisticsCalculator::ConnectomicsStatisticsCalculator()
  : m_Network( nullptr )
  , m_NumberOfVertices( 0 )
  , m_NumberOfEdges( 0 )
  , m_AverageDegree( 0.0 )
  , m_ConnectionDensity( 0.0 )
  , m_NumberOfConnectedComponents( 0 )
  , m_AverageComponentSize( 0.0 )
  , m_Components(0)
  , m_LargestComponentSize( 0 )
  , m_HopPlotExponent( 0.0 )
  , m_EffectiveHopDiameter( 0.0 )
  , m_VectorOfClusteringCoefficientsC( 0 )
  , m_VectorOfClusteringCoefficientsD( 0 )
  , m_VectorOfClusteringCoefficientsE( 0 )
  , m_AverageClusteringCoefficientsC( 0.0 )
  , m_AverageClusteringCoefficientsD( 0.0 )
  , m_AverageClusteringCoefficientsE( 0.0 )
  , m_VectorOfVertexBetweennessCentralities( 0 )
  , m_PropertyMapOfVertexBetweennessCentralities( )
  , m_AverageVertexBetweennessCentrality( 0.0 )
  , m_VectorOfEdgeBetweennessCentralities( 0 )
  , m_PropertyMapOfEdgeBetweennessCentralities( )
  , m_AverageEdgeBetweennessCentrality( 0.0 )
  , m_NumberOfIsolatedPoints( 0 )
  , m_RatioOfIsolatedPoints( 0.0 )
  , m_NumberOfEndPoints( 0 )
  , m_RatioOfEndPoints( 0.0 )
  , m_VectorOfEccentrities( 0 )
  , m_VectorOfEccentrities90( 0 )
  , m_VectorOfAveragePathLengths( 0.0 )
  , m_Diameter( 0 )
  , m_Diameter90( 0 )
  , m_Radius( 0 )
  , m_Radius90( 0 )
  , m_AverageEccentricity( 0.0 )
  , m_AverageEccentricity90( 0.0 )
  , m_AveragePathLength( 0.0 )
  , m_NumberOfCentralPoints( 0 )
  , m_RatioOfCentralPoints( 0.0 )
  , m_VectorOfSortedEigenValues( 0 )
  , m_SpectralRadius( 0.0 )
  , m_SecondLargestEigenValue( 0.0 )
  , m_AdjacencyTrace( 0.0 )
  , m_AdjacencyEnergy( 0.0 )
  , m_VectorOfSortedLaplacianEigenValues( 0 )
  , m_LaplacianTrace( 0.0 )
  , m_LaplacianEnergy( 0.0 )
  , m_LaplacianSpectralGap( 0.0 )
  , m_VectorOfSortedNormalizedLaplacianEigenValues( 0 )
  , m_NormalizedLaplacianTrace( 0.0 )
  , m_NormalizedLaplacianEnergy( 0.0 )
  , m_NormalizedLaplacianNumberOf2s( 0 )
  , m_NormalizedLaplacianNumberOf1s( 0 )
  , m_NormalizedLaplacianNumberOf0s( 0 )
  , m_NormalizedLaplacianLowerSlope( 0.0 )
  , m_NormalizedLaplacianUpperSlope( 0.0 )
  , m_SmallWorldness( 0.0 )
{
}

mitk::ConnectomicsStatisticsCalculator::~ConnectomicsStatisticsCalculator()
{
}

void mitk::ConnectomicsStatisticsCalculator::Update()
{
  CalculateNumberOfVertices();
  CalculateNumberOfEdges();
  CalculateAverageDegree();
  CalculateConnectionDensity();
  CalculateNumberOfConnectedComponents();
  CalculateAverageComponentSize();
  CalculateLargestComponentSize();
  CalculateRatioOfNodesInLargestComponent();
  CalculateHopPlotValues();
  CalculateClusteringCoefficients();
  CalculateBetweennessCentrality();
  CalculateIsolatedAndEndPoints();
  CalculateShortestPathMetrics();
  CalculateSpectralMetrics();
  CalculateLaplacianMetrics();
  CalculateNormalizedLaplacianMetrics();
  CalculateSmallWorldness();
}

void mitk::ConnectomicsStatisticsCalculator::CalculateNumberOfVertices()
{
  m_NumberOfVertices = boost::num_vertices( *(m_Network->GetBoostGraph()) );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateNumberOfEdges()
{
  m_NumberOfEdges = boost::num_edges(  *(m_Network->GetBoostGraph()) );
}

void  mitk::ConnectomicsStatisticsCalculator::CalculateAverageDegree()
{
  m_AverageDegree = ( ( (double) m_NumberOfEdges * 2.0 ) / (double) m_NumberOfVertices );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateConnectionDensity()
{
  double numberOfPossibleEdges = (double) m_NumberOfVertices * ( (double) m_NumberOfVertices - 1 ) / 2;

  m_ConnectionDensity = (double) m_NumberOfEdges / numberOfPossibleEdges;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateNumberOfConnectedComponents()
{
  m_Components.resize( m_NumberOfVertices );
  m_NumberOfConnectedComponents = boost::connected_components( *(m_Network->GetBoostGraph()), &m_Components[0] );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateAverageComponentSize()
{
  m_AverageComponentSize = (double) m_NumberOfVertices / (double) m_NumberOfConnectedComponents ;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateLargestComponentSize()
{
  m_LargestComponentSize = 0;
  std::vector<unsigned int> bins( m_NumberOfConnectedComponents );

  for(unsigned int i=0; i < m_NumberOfVertices; i++)
  {
    bins[ m_Components[i] ]++;
  }

  for(unsigned int i=0; i < m_NumberOfConnectedComponents; i++)
  {
    if (bins[i] > m_LargestComponentSize )
    {
      m_LargestComponentSize = bins[i];
    }
  }
}

void mitk::ConnectomicsStatisticsCalculator::CalculateRatioOfNodesInLargestComponent()
{
  m_RatioOfNodesInLargestComponent = (double) m_LargestComponentSize / (double) m_NumberOfVertices ;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateHopPlotValues()
{
  std::vector<int> bins( m_NumberOfVertices );

  VertexIteratorType vi, vi_end;
  unsigned int index( 0 );

  for( boost::tie( vi, vi_end ) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi != vi_end; ++vi)
  {
    std::vector<int> distances(m_NumberOfVertices, 0);
    int max_distance = 0;
    VertexDescriptorType src = *vi;
    distances[src] = 0;
    unsigned int size = 0;

    boost::breadth_first_search(*(m_Network->GetBoostGraph()), src,
      visitor(all_pairs_shortest_recorder< NetworkType >
      (&distances[0], max_distance, size)));

    for(index=0; index < distances.size(); index++)
    {
      if(distances[index] > 0)
      {
        bins[distances[index]]++;
      }
    }
  }

  bins[0] = m_NumberOfVertices;
  for(index=1; index < bins.size(); index++)
  {
    bins[index] = bins[index] + bins[index-1];
  }

  int counter = 0;
  double C=0, D=0, E=0, F=0;

  for (unsigned int i=1; i<bins.size()-1; i++)
  {
    counter ++;
    if(fabs(log(double(bins[i+1])) - log(double(bins[i]))) < 0.0000001)
    {
      break;
    }
  }

  for (int i=1; i<=counter; i++)
  {
    double x = log(double(i));
    double y = log(double(bins[i]));
    C += x;
    D += y;
    E += x * y;
    F += x * x;
  }
  double b = (D*F - C*E)/(F*counter - C*C);
  m_HopPlotExponent = (E - b*C)/F;

  m_EffectiveHopDiameter =
    std::pow( ( m_NumberOfVertices * m_NumberOfVertices )
    / ( m_NumberOfVertices + 2 * m_NumberOfEdges ), 1.0 / m_HopPlotExponent );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateClusteringCoefficients()
{
  VertexIteratorType vi, vi_end;
  std::vector<double> m_VectorOfClusteringCoefficientsC;
  std::vector<double> m_VectorOfClusteringCoefficientsD;
  std::vector<double> m_VectorOfClusteringCoefficientsE;
  typedef std::set<VertexDescriptorType> NeighborSetType;
  typedef NetworkType::out_edge_iterator OutEdgeIterType;

  for(boost::tie(vi,vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    // Get the list of vertices which are in the neighborhood of vi.
    std::pair<AdjacencyIteratorType, AdjacencyIteratorType> adjacent =
      boost::adjacent_vertices(*vi, *(m_Network->GetBoostGraph()) );

    //Populate a set with the neighbors of vi
    NeighborSetType neighbors;
    for(; adjacent.first!=adjacent.second; ++adjacent.first)
    {
      neighbors.insert(*adjacent.first);
    }

    // Now, count the edges between vertices in the neighborhood.
    unsigned int neighborhood_edge_count = 0;
    if(neighbors.size() > 0)
    {
      NeighborSetType::iterator iter;
      for(iter = neighbors.begin(); iter != neighbors.end(); ++iter)
      {
        std::pair<OutEdgeIterType, OutEdgeIterType> oe = out_edges(*iter, *(m_Network->GetBoostGraph()) );
        for(; oe.first != oe.second; ++oe.first)
        {
          if(neighbors.find(target(*oe.first, *(m_Network->GetBoostGraph()) )) != neighbors.end())
          {
            ++neighborhood_edge_count;
          }
        }
      }
      neighborhood_edge_count /= 2;
    }
    //Clustering Coefficienct C,E
    if(neighbors.size() > 1)
    {
      double num   = neighborhood_edge_count;
      double denum = neighbors.size() * (neighbors.size()-1)/2;
      m_VectorOfClusteringCoefficientsC.push_back( num / denum);
      m_VectorOfClusteringCoefficientsE.push_back( num / denum);
    }
    else
    {
      m_VectorOfClusteringCoefficientsC.push_back(0.0);
    }

    //Clustering Coefficienct D
    if(neighbors.size() > 0)
    {
      double num   = neighbors.size() + neighborhood_edge_count;
      double denum = ( (neighbors.size()+1) * neighbors.size()) / 2;
      m_VectorOfClusteringCoefficientsD.push_back( num / denum);
    }
    else
    {
      m_VectorOfClusteringCoefficientsD.push_back(0.0);
    }
  }

  // Average Clustering coefficienies:
  m_AverageClusteringCoefficientsC = std::accumulate(m_VectorOfClusteringCoefficientsC.begin(),
    m_VectorOfClusteringCoefficientsC.end(),
    0.0) / m_NumberOfVertices;

  m_AverageClusteringCoefficientsD = std::accumulate(m_VectorOfClusteringCoefficientsD.begin(),
    m_VectorOfClusteringCoefficientsD.end(),
    0.0) / m_NumberOfVertices;

  m_AverageClusteringCoefficientsE = std::accumulate(m_VectorOfClusteringCoefficientsE.begin(),
    m_VectorOfClusteringCoefficientsE.end(),
    0.0) / m_VectorOfClusteringCoefficientsE.size();
}

void mitk::ConnectomicsStatisticsCalculator::CalculateBetweennessCentrality()
{
  // std::map used for convenient initialization
  EdgeIndexStdMapType stdEdgeIndex;
  // associative property map needed for iterator property map-wrapper
  EdgeIndexMapType edgeIndex(stdEdgeIndex);

  EdgeIteratorType iterator, end;

  // sets iterator to start end end to end
  boost::tie(iterator, end) = boost::edges( *(m_Network->GetBoostGraph()) );

  int i(0);
  for ( ; iterator != end; ++iterator, ++i)
  {
    stdEdgeIndex.insert(std::pair< EdgeDescriptorType, int >( *iterator, i));
  }

  // Define EdgeCentralityMap
  m_VectorOfEdgeBetweennessCentralities.resize( m_NumberOfEdges, 0.0);
  // Create the external property map
  m_PropertyMapOfEdgeBetweennessCentralities = EdgeIteratorPropertyMapType(m_VectorOfEdgeBetweennessCentralities.begin(), edgeIndex);

  // Define VertexCentralityMap
  VertexIndexMapType vertexIndex = get(boost::vertex_index, *(m_Network->GetBoostGraph()) );
  m_VectorOfVertexBetweennessCentralities.resize( m_NumberOfVertices, 0.0);
  // Create the external property map
  m_PropertyMapOfVertexBetweennessCentralities = VertexIteratorPropertyMapType(m_VectorOfVertexBetweennessCentralities.begin(), vertexIndex);

  boost::brandes_betweenness_centrality( *(m_Network->GetBoostGraph()),
    m_PropertyMapOfVertexBetweennessCentralities, m_PropertyMapOfEdgeBetweennessCentralities );

  m_AverageVertexBetweennessCentrality = std::accumulate(m_VectorOfVertexBetweennessCentralities.begin(),
    m_VectorOfVertexBetweennessCentralities.end(),
    0.0) / (double) m_NumberOfVertices;

  m_AverageEdgeBetweennessCentrality = std::accumulate(m_VectorOfEdgeBetweennessCentralities.begin(),
    m_VectorOfEdgeBetweennessCentralities.end(),
    0.0) / (double) m_NumberOfEdges;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateIsolatedAndEndPoints()
{
  m_NumberOfIsolatedPoints = 0;
  m_NumberOfEndPoints = 0;

  VertexIteratorType vi, vi_end;
  for( boost::tie(vi,vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    int degree = boost::out_degree(*vi, *(m_Network->GetBoostGraph()) );
    if(degree == 0)
    {
      m_NumberOfIsolatedPoints++;
    }
    else if (degree == 1)
    {
      m_NumberOfEndPoints++;
    }
  }

  m_RatioOfEndPoints = (double) m_NumberOfEndPoints / (double) m_NumberOfVertices;
  m_RatioOfIsolatedPoints = (double) m_NumberOfIsolatedPoints / (double) m_NumberOfVertices;
}

/**
* Calculates Shortest Path Related metrics of the graph.  The
* function runs a BFS from each node to find out the shortest
* distances to other nodes in the graph. The maximum of this distance
* is called the eccentricity of that node. The maximum eccentricity
* in the graph is called diameter and the minimum eccentricity is
* called the radius of the graph.  Central points are those nodes
* having eccentricity equals to radius.
*/
void mitk::ConnectomicsStatisticsCalculator::CalculateShortestPathMetrics()
{
  //for all vertices:
  VertexIteratorType vi, vi_end;

  //store the eccentricities in a vector.
  m_VectorOfEccentrities.resize( m_NumberOfVertices );
  m_VectorOfEccentrities90.resize( m_NumberOfVertices );
  m_VectorOfAveragePathLengths.resize( m_NumberOfVertices );

  //assign diameter and radius while iterating over the ecccencirities.
  m_Diameter              = 0;
  m_Diameter90            = 0;
  m_Radius                = std::numeric_limits<unsigned int>::max();
  m_Radius90              = std::numeric_limits<unsigned int>::max();
  m_AverageEccentricity   = 0.0;
  m_AverageEccentricity90 = 0.0;
  m_AveragePathLength     = 0.0;

  //The size of the giant connected component so far.
  unsigned int giant_component_size = 0;
  VertexDescriptorType radius_src(0);

  //Loop over the vertices
  for( boost::tie(vi, vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    //We are going to start a BFS, initialize the neccessary
    //structures for that.  Store the distances of nodes from the
    //source in distance vector. The maximum distance is stored in
    //max. The BFS will start from the node that vi is pointing, that
    //is the src is *vi. We also init the distance of the src node to
    //itself to 0. size gives the number of nodes discovered during
    //this BFS.
    std::vector<int> distances( m_NumberOfVertices );
    int max_distance = 0;
    VertexDescriptorType src = *vi;
    distances[src] = 0;
    unsigned int size = 0;

    breadth_first_search(*(m_Network->GetBoostGraph()), src,
      visitor(all_pairs_shortest_recorder<NetworkType>
      (&distances[0], max_distance, size)));
    // vertex vi has eccentricity equal to max_distance
    m_VectorOfEccentrities[src] = max_distance;

    //check whether there is any change in the diameter or the radius.
    //note that the diameter we are calculating here is also the
    //diameter of the giant connected component!
    if(m_VectorOfEccentrities[src] > m_Diameter)
    {
      m_Diameter = m_VectorOfEccentrities[src];
    }

    //The radius should be calculated on the largest connected
    //component, otherwise it is very likely that radius will be 1.
    //We change the value of the radius only if this is the giant
    //connected component so far. After all the eccentricities are
    //found we should loop over this connected component and find the
    //minimum eccentricity which is the radius. So we keep the src
    //node, so that we can find the connected component later on.
    if(size > giant_component_size)
    {
      giant_component_size = size;
      radius_src = src;
    }

    //Calculate in how many hops we can reach 90 percent of the
    //nodes. We store the number of hops we can reach in h hops in the
    //bucket vector. That is bucket[h] gives the number of nodes
    //reachable in exactly h hops. sum of bucket[i<h] gives the number
    //of nodes that are reachable in less than h hops. We also
    //calculate sum of the distances from this node to every single
    //other node in the graph.
    int reachable90 = std::ceil((double)size * 0.9);
    std::vector <int> bucket (max_distance+1);
    int counter = 0;
    for(unsigned int i=0; i<distances.size(); i++)
    {
      if(distances[i]>0)
      {
        bucket[distances[i]]++;
        m_VectorOfAveragePathLengths[src] += distances[i];
        counter ++;
      }
    }
    if(counter > 0)
    {
      m_VectorOfAveragePathLengths[src] = m_VectorOfAveragePathLengths[src] / counter;
    }

    int eccentricity90 = 0;
    while(reachable90 > 0)
    {
      eccentricity90 ++;
      reachable90 = reachable90 - bucket[eccentricity90];
    }
    // vertex vi has eccentricity90 equal to eccentricity90
    m_VectorOfEccentrities90[src] = eccentricity90;
    if(m_VectorOfEccentrities90[src] > m_Diameter90)
    {
      m_Diameter90 = m_VectorOfEccentrities90[src];
    }
  }

  //We are going to calculate the radius now. We stored the src node
  //that when we start a BFS gives the giant connected component, and
  //we have the eccentricities calculated. Iterate over the nodes of
  //this giant component and find the minimum eccentricity.
  std::vector<int> component( m_NumberOfVertices );
  boost::connected_components( *(m_Network->GetBoostGraph()), &component[0]);
  for (unsigned int i=0; i<component.size(); i++)
  {
    //If we are in the same component and the radius is not the
    //minimum so far store the eccentricity as the radius.
    if( component[i] == component[radius_src])
    {
      if(m_Radius > m_VectorOfEccentrities[i])
      {
        m_Radius = m_VectorOfEccentrities[i];
      }
      if(m_Radius90 > m_VectorOfEccentrities90[i])
      {
        m_Radius90 = m_VectorOfEccentrities90[i];
      }
    }
  }

  m_AverageEccentricity = std::accumulate(m_VectorOfEccentrities.begin(),
    m_VectorOfEccentrities.end(), 0.0) / m_NumberOfVertices;

  m_AverageEccentricity90 = std::accumulate(m_VectorOfEccentrities90.begin(),
    m_VectorOfEccentrities90.end(), 0.0) / m_NumberOfVertices;

  m_AveragePathLength = std::accumulate(m_VectorOfAveragePathLengths.begin(),
    m_VectorOfAveragePathLengths.end(), 0.0) / m_NumberOfVertices;

  //calculate Number of Central Points, nodes having eccentricity = radius.
  m_NumberOfCentralPoints = 0;
  for (boost::tie(vi, vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi != vi_end; ++vi)
  {
    if(m_VectorOfEccentrities[*vi] == m_Radius)
    {
      m_NumberOfCentralPoints++;
    }
  }
  m_RatioOfCentralPoints = (double)m_NumberOfCentralPoints / m_NumberOfVertices;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateSpectralMetrics()
{
  mitk::ConnectomicsNetworkConverter::Pointer converter = mitk::ConnectomicsNetworkConverter::New();
  converter->SetNetwork( m_Network );
  vnl_matrix<double> adjacencyMatrix = converter->GetNetworkAsVNLAdjacencyMatrix();

  vnl_symmetric_eigensystem<double> eigenSystem(adjacencyMatrix);

  m_AdjacencyTrace = 0;
  m_AdjacencyEnergy = 0;
  m_VectorOfSortedEigenValues.clear();

  for(unsigned int i=0; i < m_NumberOfVertices; ++i)
  {
    double value = std::fabs(eigenSystem.get_eigenvalue(i));
    m_VectorOfSortedEigenValues.push_back(value);
    m_AdjacencyTrace += value;
    m_AdjacencyEnergy += value * value;
  }

  std::sort(m_VectorOfSortedEigenValues.begin(), m_VectorOfSortedEigenValues.end());

  m_SpectralRadius = m_VectorOfSortedEigenValues[ m_NumberOfVertices - 1];
  m_SecondLargestEigenValue  = m_VectorOfSortedEigenValues[ m_NumberOfVertices - 2];
}

void mitk::ConnectomicsStatisticsCalculator::CalculateLaplacianMetrics()
{
  mitk::ConnectomicsNetworkConverter::Pointer converter = mitk::ConnectomicsNetworkConverter::New();
  converter->SetNetwork( m_Network );
  vnl_matrix<double> adjacencyMatrix = converter->GetNetworkAsVNLAdjacencyMatrix();
  vnl_matrix<double> laplacianMatrix ( m_NumberOfVertices, m_NumberOfVertices, 0);
  vnl_matrix<double> degreeMatrix = converter->GetNetworkAsVNLDegreeMatrix();

  m_VectorOfSortedLaplacianEigenValues.clear();
  laplacianMatrix = degreeMatrix - adjacencyMatrix;
  int numberOfConnectedComponents = 0;
  vnl_symmetric_eigensystem <double> laplacianEigenSystem( laplacianMatrix );
  m_LaplacianEnergy = 0;
  m_LaplacianTrace  = 0;
  for(unsigned int i(0); i < m_NumberOfVertices; ++i)
  {
    double value = std::fabs( laplacianEigenSystem.get_eigenvalue(i) );
    m_VectorOfSortedLaplacianEigenValues.push_back( value );
    m_LaplacianTrace += value;
    m_LaplacianEnergy += value * value;
    if ( std::fabs( value ) < mitk::eps )
    {
      numberOfConnectedComponents++;
    }
  }

  std::sort(m_VectorOfSortedLaplacianEigenValues.begin(), m_VectorOfSortedLaplacianEigenValues.end());
  for(unsigned int i(0); i < m_VectorOfSortedLaplacianEigenValues.size(); ++i)
  {
    if(m_VectorOfSortedLaplacianEigenValues[i] > mitk::eps )
    {
      m_LaplacianSpectralGap = m_VectorOfSortedLaplacianEigenValues[i];
      break;
    }
  }
}

void  mitk::ConnectomicsStatisticsCalculator::CalculateNormalizedLaplacianMetrics()
{
  vnl_matrix<double> normalizedLaplacianMatrix(m_NumberOfVertices, m_NumberOfVertices, 0);
  EdgeIteratorType ei, ei_end;
  VertexDescriptorType sourceVertex, destinationVertex;
  int sourceIndex, destinationIndex;
  VertexIndexMapType vertexIndexMap = boost::get(boost::vertex_index, *(m_Network->GetBoostGraph()) );
  m_VectorOfSortedNormalizedLaplacianEigenValues.clear();

  // Normalized laplacian matrix
  for( boost::tie(ei, ei_end) = boost::edges( *(m_Network->GetBoostGraph()) ); ei != ei_end; ++ei)
  {
    sourceVertex = boost::source(*ei, *(m_Network->GetBoostGraph()) );
    sourceIndex = vertexIndexMap[sourceVertex];

    destinationVertex = boost::target(*ei, *(m_Network->GetBoostGraph()) );
    destinationIndex = vertexIndexMap[destinationVertex];
    int sourceDegree = boost::out_degree(sourceVertex, *(m_Network->GetBoostGraph()) );
    int destinationDegree = boost::out_degree(destinationVertex, *(m_Network->GetBoostGraph()) );

    normalizedLaplacianMatrix.put(
      sourceIndex, destinationIndex, -1 / (sqrt(double(sourceDegree * destinationDegree))));
    normalizedLaplacianMatrix.put(
      destinationIndex, sourceIndex, -1 / (sqrt(double(sourceDegree * destinationDegree))));
  }

  VertexIteratorType vi, vi_end;
  for(boost::tie(vi, vi_end)=boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    if(boost::out_degree(*vi, *(m_Network->GetBoostGraph()) ) > 0)
    {
      normalizedLaplacianMatrix.put(vertexIndexMap[*vi], vertexIndexMap[*vi], 1);
    }
  }
  //End of normalized laplacian matrix definition

  vnl_symmetric_eigensystem <double>
    normalizedLaplacianEigensystem(normalizedLaplacianMatrix);

  double N1=0, C1=0, D1=0, E1=0, F1=0, b1=0;
  double N2=0, C2=0, D2=0, E2=0, F2=0, b2=0;
  m_NormalizedLaplacianNumberOf2s = 0;
  m_NormalizedLaplacianNumberOf1s = 0;
  m_NormalizedLaplacianNumberOf0s = 0;
  m_NormalizedLaplacianTrace = 0;
  m_NormalizedLaplacianEnergy = 0;

  for(unsigned int i(0); i< m_NumberOfVertices; ++i)
  {
    double eigenValue = std::fabs(normalizedLaplacianEigensystem.get_eigenvalue(i));
    m_VectorOfSortedNormalizedLaplacianEigenValues.push_back(eigenValue);
    m_NormalizedLaplacianTrace  += eigenValue;
    m_NormalizedLaplacianEnergy += eigenValue * eigenValue;

    //0
    if(eigenValue < mitk::eps)
    {
      m_NormalizedLaplacianNumberOf0s++;
    }

    //Between 0 and 1.
    else if(eigenValue > mitk::eps && eigenValue< 1 - mitk::eps)
    {
      C1 += i;
      D1 += eigenValue;
      E1 += i * eigenValue;
      F1 += i * i;
      N1 ++;
    }

    //1
    else if(std::fabs( std::fabs(eigenValue) - 1) < mitk::eps)
    {
      m_NormalizedLaplacianNumberOf1s++;
    }

    //Between 1 and 2
    else if(std::fabs(eigenValue) > 1+mitk::eps && std::fabs(eigenValue)< 2 - mitk::eps)
    {
      C2 += i;
      D2 += eigenValue;
      E2 += i * eigenValue;
      F2 += i * i;
      N2 ++;
    }

    //2
    else if(std::fabs( std::fabs(eigenValue) - 2) < mitk::eps)
    {
      m_NormalizedLaplacianNumberOf2s++;
    }
  }

  b1 = (D1*F1 - C1*E1)/(F1*N1 - C1*C1);
  m_NormalizedLaplacianLowerSlope = (E1 - b1*C1)/F1;

  b2 = (D2*F2 - C2*E2)/(F2*N2 - C2*C2);
  m_NormalizedLaplacianUpperSlope = (E2 - b2*C2)/F2;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateSmallWorldness()
{
  double k( this->GetAverageDegree() );
  double N( this->GetNumberOfVertices() );
  // The clustering coefficient of an Erdos-Reny network is equivalent to
  // the likelihood two random nodes are connected
  double gamma = this->GetAverageClusteringCoefficientsC() / ( k / N );
  //The mean path length of an Erdos-Reny network is approximately
  // ln( #vertices ) / ln( average degree )
  double lambda = this->GetAveragePathLength() / ( std::log( N ) / std::log( k ) );

  m_SmallWorldness = gamma / lambda;
}


#include "Arduino.h"
#include "navduino.h"
#include "eigen.h"
#include <Eigen/Geometry>

using namespace Eigen;

void printVec2f(const Vector2f& vec, const int& p, Stream& stream)
{
	stream.println(vec(0), p);
	stream.println(vec(1), p);
}

void printVec3f(const Vector3f& vec, const int& p, Stream& stream)
{
	Serial.println(vec(0), p);
	Serial.println(vec(1), p);
	Serial.println(vec(2), p);
}

void printVec4f(const Vector4f& vec, const int& p, Stream& stream)
{
	Serial.println(vec(0), p);
	Serial.println(vec(1), p);
	Serial.println(vec(2), p);
	Serial.println(vec(3), p);
}

void printQuatf(const Quaternionf& quat, const int& p, Stream& stream)
{
	Serial.println(quat.x(), p);
	Serial.println(quat.y(), p);
	Serial.println(quat.z(), p);
	Serial.println(quat.w(), p);
}

void printMat3f(const Matrix3f& mat, const int& p, Stream& stream)
{
	Serial.print(mat(0, 0), p); Serial.print(", "); Serial.print(mat(0, 1), p); Serial.print(", "); Serial.println(mat(0, 2), p);
	Serial.print(mat(1, 0), p); Serial.print(", "); Serial.print(mat(1, 1), p); Serial.print(", "); Serial.println(mat(1, 2), p);
	Serial.print(mat(2, 0), p); Serial.print(", "); Serial.print(mat(2, 1), p); Serial.print(", "); Serial.println(mat(2, 2), p);
}

void printMat4f(const Matrix4f& mat, const int& p, Stream& stream)
{
	Serial.print(mat(0, 0), p); Serial.print(", "); Serial.print(mat(0, 1), p); Serial.print(", "); Serial.print(mat(0, 2), p); Serial.print(", "); Serial.println(mat(0, 3), p);
	Serial.print(mat(1, 0), p); Serial.print(", "); Serial.print(mat(1, 1), p); Serial.print(", "); Serial.print(mat(1, 2), p); Serial.print(", "); Serial.println(mat(1, 3), p);
	Serial.print(mat(2, 0), p); Serial.print(", "); Serial.print(mat(2, 1), p); Serial.print(", "); Serial.print(mat(2, 2), p); Serial.print(", "); Serial.println(mat(2, 3), p);
	Serial.print(mat(3, 0), p); Serial.print(", "); Serial.print(mat(3, 1), p); Serial.print(", "); Serial.print(mat(3, 2), p); Serial.print(", "); Serial.println(mat(3, 3), p);
}

float float_constrain(const float& input, const float& min, const float& max)
{
	if (input > max)
		return max;
	else if (input < min)
		return min;
	else
		return input;
}

double double_constrain(const double& input, const double& min, const double& max)
{
	if (input > max)
		return max;
	else if (input < min)
		return min;
	else
		return input;
}

float float_map(const float& x, const float& in_min, const float& in_max, const float& out_min, const float& out_max)
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

double double_map(const double& x, const double& in_min, const double& in_max, const double& out_min, const double& out_max)
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


//          Copyright (C) 2012, Michele Caini.
// 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)

//          Two Graphs Common Spanning Trees Algorithm
//      Based on academic article of Mint, Read and Tarjan
//     Efficient Algorithm for Common Spanning Tree Problem
// Electron. Lett., 28 April 1983, Volume 19, Issue 9, p.346-347

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/two_graphs_common_spanning_trees.hpp>
#include <exception>
#include <vector>

using namespace std;

typedef
boost::adjacency_list
  <
    boost::vecS,         // OutEdgeList
    boost::vecS,         // VertexList
    boost::undirectedS,  // Directed
    boost::no_property,  // VertexProperties
    boost::no_property,  // EdgeProperties
    boost::no_property,  // GraphProperties
    boost::listS         // EdgeList
  >
Graph
;

typedef
boost::graph_traits<Graph>::vertex_descriptor
vertex_descriptor;

typedef
boost::graph_traits<Graph>::edge_descriptor
edge_descriptor;

typedef
boost::graph_traits<Graph>::vertex_iterator
vertex_iterator;

typedef
boost::graph_traits<Graph>::edge_iterator
edge_iterator;

int main(int argc, char **argv)
{
  Graph iG, vG;
  vector< edge_descriptor > iG_o;
  vector< edge_descriptor > vG_o;

  iG_o.push_back(boost::add_edge(0, 1, iG).first);
  iG_o.push_back(boost::add_edge(0, 2, iG).first);
  iG_o.push_back(boost::add_edge(0, 3, iG).first);
  iG_o.push_back(boost::add_edge(0, 4, iG).first);
  iG_o.push_back(boost::add_edge(1, 2, iG).first);
  iG_o.push_back(boost::add_edge(3, 4, iG).first);

  vG_o.push_back(boost::add_edge(1, 2, vG).first);
  vG_o.push_back(boost::add_edge(2, 0, vG).first);
  vG_o.push_back(boost::add_edge(2, 3, vG).first);
  vG_o.push_back(boost::add_edge(4, 3, vG).first);
  vG_o.push_back(boost::add_edge(0, 3, vG).first);
  vG_o.push_back(boost::add_edge(0, 4, vG).first);

  vector<bool> inL(iG_o.size(), false);

  std::vector< std::vector<bool> > coll;
  boost::tree_collector<
      std::vector< std::vector<bool> >,
      std::vector<bool>
    > tree_collector(coll);
  boost::two_graphs_common_spanning_trees
    (
      iG,
      iG_o,
      vG,
      vG_o,
      tree_collector,
      inL
    );

  std::vector< std::vector<bool> >::iterator it;
  for(it = coll.begin(); it != coll.end(); ++it) {
    // Here you can play with the trees that the algorithm has found.
  }

  return 0;
}


#ifndef SPLINE_H
#define SPLINE_H

//#pragma GCC optimize("O3")

#include "polynomial.hpp"
#include <Eigen/Dense>

template <unsigned Deg> class Spline
{
private:
	using MatrixDXf = Eigen::Matrix<float,Deg+1,Eigen::Dynamic>;
	using MatrixXDf = Eigen::Matrix<float,Eigen::Dynamic,Deg+1>;
	MatrixDXf c;
	float xmin, xmax;
	float invdx;
	Spline() = default;
	void setFromSorted(const Eigen::Matrix2Xf &sortedXY,
			   const unsigned NPieces);
	inline float value(int i, float x) const;
	static Eigen::VectorXf values(const Eigen::VectorXf& x, const MatrixXDf& cPerm);

public:
	inline float value_unsafe(const float &x) const
	{
		int i = (x - xmin) * invdx;
		assert(i >= 0);
		assert(i < c.cols());
		return value(i,x);
	}
	float value_or(const float &x, const float &defaultValue) const
	{
		if (x < xmin || x > xmax) {
			return defaultValue;
		}
		int i = (x - xmin) * invdx;
		return value(i,x);
	}
	float value_or(const float &x, const float defaultLeft, const float defaultRight) const
	{
		if (x < xmin) {
			return defaultLeft;
		}
		if (x > xmax) {
			return defaultRight;
		}
		int i = (x - xmin) * invdx;
		return value(i,x);
	}
	Eigen::VectorXf values_unsafe(const Eigen::VectorXf& x) const;

	unsigned maxAbsDiffArg(const Eigen::Matrix2Xf &xy) const
	{
		float max=0.0f;
		unsigned arg=0;
		for(unsigned i=0; i<xy.cols(); ++i) {
			float diff=std::fabs(xy(1,i)-value_unsafe(xy(0,i)));
			if(max<diff) {
				max=diff;
				arg=i;
			}
		}
		return arg;
	}
	static Spline<Deg> fromSorted(const Eigen::Matrix2Xf &sortedXY,
				      const unsigned NPieces);

	template <unsigned Nd>
	friend std::ostream &operator<<(std::ostream &os,
					const Spline<Nd> &spline);
};
template <unsigned Deg>
std::ostream &operator<<(std::ostream &os, const Spline<Deg> &spline)
{
	for (int i = 0; i < spline.polynomials.size(); ++i) {
		float dx = 1.0 / spline.invdx;
		float from = spline.xmin + dx * i;
		float to = from + dx;
		os << '[' << from << ".." << to << "] " << spline.polynomials[i]
		   << '\n';
	}
	return os;
}

template <unsigned int Deg>
Spline<Deg> Spline<Deg>::fromSorted(const Eigen::Matrix2Xf &sortedXY,
				    const unsigned NPieces)
{
	Spline<Deg> sp;
	sp.setFromSorted(sortedXY, NPieces);
	return sp;
}
template <unsigned int Deg>
void Spline<Deg>::setFromSorted(const Eigen::Matrix2Xf &sortedXY,
				const unsigned NPieces)
{
	const int nPoints = sortedXY.cols();
	assert(nPoints > Deg);

	xmin = sortedXY(0, 0);
	xmax = sortedXY(0, nPoints - 1);
	constexpr float inf = std::numeric_limits<float>::infinity();
	const float dx = nextafterf((xmax - xmin) / NPieces, inf);
	invdx = 1.0f / dx;

	assert((xmax - xmin) * invdx < NPieces);

	c.resize(3,NPieces);
	int iStart = 0;
	for (int piece = 0; piece < NPieces - 1; ++piece) {
		float xEnd = dx * (piece + 1) + xmin;
		int iEnd = iStart;
		while (sortedXY(0, iEnd) < xEnd) {
			++iEnd;
		}
		int len = std::max(iEnd - iStart, int(Deg + 1));
		Polynomial<Deg> p(sortedXY.middleCols(iStart, len));
		c.col(piece) = p.c;
		if (iEnd > iStart + Deg) {
			iStart = std::min(int(nPoints - Deg - 1), iEnd);
		}
	}

	Polynomial<Deg> p(sortedXY.middleCols(iStart, nPoints - iStart));
	c.col(NPieces - 1) = p.c;
}

template<>
float Spline<0>::value(int i, float ) const
{
	return c(0,i);
}

template<>
float Spline<1>::value(int i, float x) const
{
	return c(0,i)+c(1,i)*x;
}

template<>
float Spline<2>::value(int i, float x) const
{
	return c(0,i)+c(1,i)*x+c(2,i)*x*x;
}

template<unsigned int Deg>
float Spline<Deg>::value(int i, float x) const
{
	float val=c(0,i)+c(1,i)*x+c(2,i)*x*x;
	for (int e=3; e<=Deg; e++) {
		val+=c(e,i)*std::pow(x,e);
	}
	return val;
}

template<unsigned int Deg>
Eigen::VectorXf Spline<Deg>::values_unsafe(const Eigen::VectorXf& x) const
{
	//x<xmin is not verified at runtime!
	assert(x.minCoeff()>=xmin);
	const int maxCol=c.cols()-1;
	const unsigned N=x.size();

	Eigen::VectorXi idxs=((x.array()-xmin)*invdx).cast<int>().min(maxCol);
	MatrixXDf slc(N,Deg+1);
	for(int i=0; i<N; ++i) {
		slc.row(i) = c.col(idxs(i));
	}
	return values(x,slc);
}

template<unsigned int Deg>
Eigen::VectorXf Spline<Deg>::values(const Eigen::VectorXf& x, const Spline::MatrixXDf& cPerm)
{
	Eigen::VectorXf res=Spline<2>::values(x,cPerm);
	for (int e=3; e<=Deg; ++e) {
		res+=cPerm.col(e).cwiseProduct(x.array().pow(e).matrix());
	}
	return res;
}

template<>
Eigen::VectorXf Spline<2>::values(const Eigen::VectorXf& x, const Spline::MatrixXDf& cPerm)
{
	return cPerm.col(0)+cPerm.col(1).cwiseProduct(x)+cPerm.col(2).cwiseProduct(x.cwiseAbs2());
}

template<>
Eigen::VectorXf Spline<1>::values(const Eigen::VectorXf& x, const Spline::MatrixXDf& cPerm)
{
	return cPerm.col(0)+cPerm.col(1).cwiseProduct(x);
}

template<>
Eigen::VectorXf Spline<0>::values(const Eigen::VectorXf& x, const Spline::MatrixXDf& cPerm)
{
	return cPerm.col(0);
}
#endif // SPLINE_H