/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file spreadedswaptionvolatility.hpp
    \brief swaption cube defined via atm vol spreads over another cube
    \ingroup termstructures
*/

#pragma once

#include <ql/indexes/swapindex.hpp>
#include <ql/math/interpolations/interpolation2d.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/volatility/smilesection.hpp>
#include <ql/termstructures/volatility/swaption/swaptionvoldiscrete.hpp>

#include <boost/shared_ptr.hpp>

namespace QuantExt {
using namespace QuantLib;

class SpreadedSwaptionVolatility : public SwaptionVolatilityDiscrete {
public:
    /* If the base vol has smile sections which provide atm levels, these are also used to define the atm levels for the
      vol spreads. Otherwise, the atm levels for the vol spreads are computed from the swap indices passed to this
      class. The swap indices are only required / used if a) the base vol smile sections do not provide atm levels and
      b) more than one strike spread is given.  */
    SpreadedSwaptionVolatility(const Handle<SwaptionVolatilityStructure>& base, const std::vector<Period>& optionTenors,
                               const std::vector<Period>& swapTenors, const std::vector<Real>& strikeSpreads,
                               const std::vector<std::vector<Handle<Quote>>>& volSpreads,
                               const boost::shared_ptr<SwapIndex>& swapIndexBase,
                               const boost::shared_ptr<SwapIndex>& shortSwapIndexBase);

    //! \name TermStructure interface
    //@{
    DayCounter dayCounter() const override;
    Date maxDate() const override;
    Time maxTime() const override;
    const Date& referenceDate() const override;
    Calendar calendar() const override;
    Natural settlementDays() const override;
    //! \name VolatilityTermStructure interface
    //@{
    Rate minStrike() const override;
    Rate maxStrike() const override;
    //@}
    //! \name SwaptionVolatilityStructure interface
    //@{
    const Period& maxSwapTenor() const override;
    VolatilityType volatilityType() const override;
    //@}
    //! \name Observer interface
    //@{
    void deepUpdate() override;
    //@}
    const Handle<SwaptionVolatilityStructure>& baseVol();

private:
    boost::shared_ptr<SmileSection> smileSectionImpl(Time optionTime, Time swapLength) const override;
    Volatility volatilityImpl(Time optionTime, Time swapLength, Rate strike) const override;
    void performCalculations() const override;

    Handle<SwaptionVolatilityStructure> base_;
    std::vector<Real> strikeSpreads_;
    std::vector<std::vector<Handle<Quote>>> volSpreads_;
    boost::shared_ptr<SwapIndex> swapIndexBase_, shortSwapIndexBase_;
    mutable std::vector<Matrix> volSpreadValues_;
    mutable std::vector<Interpolation2D> volSpreadInterpolation_;
};

} // namespace QuantExt


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_ORMQR_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_ORMQR_HPP

#include <complex>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Apply the orthogonal transformation abtained by geqrf() to
    // a general matrix.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * ormqr() overwrites the general M by N matrix C with
     *
     *                         SIDE = 'L'     SIDE = 'R'
     *  TRANS = 'N':             Q * C          C * Q
     *  TRANS = 'C' || 'T':      Q**H * C       C * Q**H
     *
     *  where Q is a complex unitary matrix defined as the product of k
     *  elementary reflectors
     *
     *        Q = H(1) H(2) . . . H(k)
     *
     *  as returned by geqrf(). Q is of order M if SIDE = 'L' and of order N
     *  if SIDE = 'R'.
     *
     *  Workspace is organized following the arguments in the calling sequence.
     *   optimal_workspace() : for optimizing use of blas 3 kernels
     *   minimal_workspace() : minimum size of workarrays, but does not allow for optimization
     *                         of blas 3 kernels
     *   workspace( work ) where is an array that is used as auxiliary memory with the same value_type
     *                     as C.
     *                     We must have that vector_size( work ) >= matrix_size2( c )
     *                     if SIDE=='L' otherwise  vector_size( work ) >= matrix_size1( c )
     */

    namespace detail {

      inline
      void ormqr (char const side, char const trans, integer_t const m, integer_t const n,
                 integer_t const k, const float* a, integer_t const lda,
                 const float* tau, float* c,
                 integer_t const ldc, float* work, integer_t const lwork,
                 integer_t& info)
      {
        assert ( trans=='N' || trans=='T' );
        LAPACK_SORMQR (&side, &trans, &m, &n, &k,
                      a, &lda,
                      tau,
                      c, &ldc,
                      work, &lwork,
                      &info);
      }

      inline
      void ormqr (char const side, char const trans, integer_t const m, integer_t const n,
                 integer_t const k, const double* a, integer_t const lda,
                 const double* tau, double* c,
                 integer_t const ldc, double* work, integer_t const lwork,
                 integer_t& info)
      {
        assert ( trans=='N' || trans=='T' );
        LAPACK_DORMQR (&side, &trans, &m, &n, &k,
                      a, &lda,
                      tau,
                      c, &ldc,
                      work, &lwork,
                      &info);
      }

      inline
      void ormqr (char const side, char const trans, integer_t const m, integer_t const n,
                 integer_t const k, const traits::complex_f* a, integer_t const lda,
                 const traits::complex_f* tau, traits::complex_f* c,
                 integer_t const ldc, traits::complex_f* work, integer_t const lwork,
                 integer_t& info)
      {
        assert ( trans=='N' || trans=='C' );
        LAPACK_CUNMQR (&side, &trans, &m, &n, &k,
                      traits::complex_ptr(a), &lda,
                      traits::complex_ptr(tau),
                      traits::complex_ptr(c), &ldc,
                      traits::complex_ptr(work), &lwork,
                      &info);
      }

      inline
      void ormqr (char const side, char const trans, integer_t const m, integer_t const n,
                 integer_t const k, const traits::complex_d* a, integer_t const lda,
                 const traits::complex_d* tau, traits::complex_d* c,
                 integer_t const ldc, traits::complex_d* work, integer_t const lwork,
                 integer_t& info)
      {
        assert ( trans=='N' || trans=='C' );
        LAPACK_ZUNMQR (&side, &trans, &m, &n, &k,
                      traits::complex_ptr(a), &lda,
                      traits::complex_ptr(tau),
                      traits::complex_ptr(c), &ldc,
                      traits::complex_ptr(work), &lwork,
                      &info);
      }

      template <typename A, typename Tau, typename C, typename Work>
      int ormqr (char side, char trans, const A& a, const Tau& tau, C& c,
                 Work& work) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<A>::matrix_structure,
          traits::general_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<C>::matrix_structure,
          traits::general_t
        >::value));
#endif

        integer_t const m = traits::matrix_size1 (c);
        integer_t const n = traits::matrix_size2 (c);
        integer_t const k = traits::vector_size (tau);
        integer_t const lwork = traits::vector_size (work);

        assert ( side=='L' || side=='R' );
        assert ( (side=='L' ?  m >= k : n >= k ) );

        assert ( (side=='L' ?
                  m == traits::matrix_size1 (a) :
                  n == traits::matrix_size1 (a) ) );
        assert (traits::matrix_size2 (a)==k);

        assert ( (side=='L' ?
                  lwork >= n : lwork >= m ) );

        integer_t info;
        ormqr (side, trans, m, n, k,
                       traits::matrix_storage (a),
                       traits::leading_dimension (a),
                       traits::vector_storage (tau),
                       traits::matrix_storage (c),
                       traits::leading_dimension (c),
                       traits::vector_storage (work),
                       lwork,
                       info);
        return info;
      }
    } // namespace detail

    // Function that allocates temporary arrays for optimal execution time.
    template <typename A, typename Tau, typename C>
    int ormqr (char side, char trans, const A& a, const Tau& tau, C& c, optimal_workspace ) {
       typedef typename A::value_type                              value_type ;

       std::ptrdiff_t const n_w = (side=='L' ? traits::matrix_size2 (c)
                                  : traits::matrix_size1 (c) );

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,n_w*32) );

       return detail::ormqr( side, trans, a, tau, c, work );
    }

    // Function that allocates temporary arrays with minimal size
    template <typename A, typename Tau, typename C>
    int ormqr (char side, char trans, const A& a, const Tau& tau, C& c, minimal_workspace ) {
       typedef typename A::value_type                              value_type ;

       std::ptrdiff_t const n_w = (side=='L' ? traits::matrix_size2 (c)
                                  : traits::matrix_size1 (c) );

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,n_w) );

       return detail::ormqr( side, trans, a, tau, c, work );
    }

    // Function that uses auxiliary array in workspace
    // The calling sequence is ormqr(side, trans, a, tau, c, workspace( work_array ) )
    // where work_array is an array with the same value_type as a and c .
    template <typename A, typename Tau, typename C, typename Work>
    int ormqr (char side, char trans, const A& a, const Tau& tau, C& c, detail::workspace1<Work> workspace ) {
       typedef typename traits::matrix_traits<A>::value_type                              value_type ;

       return detail::ormqr( side, trans, a, tau, c, workspace.select(value_type()) );
    }

  }

}}}

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing inrad capabilities

    convert degree to radian.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = inrad(x);
    @endcode

    is similar to:

    @code
    T r = Deginrad<T>()*x;
    @endcode

    @see  indeg, Radindeg, Radindegr, Deginrad

  **/
  const boost::dispatch::functor<tag::inrad_> inrad = {};
} }
#endif

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

#endif


/**
 * \file SFMT19937.hpp
 * \brief Header for SFMT19937.
 *
 * This provides an implementation of the SIMD-oriented Fast Mersenne Twister
 * random number generator,
 *
 * SFMT19937.  See\n Mutsuo Saito,\n An Application of Finite Field: Design
 * and Implementation of 128-bit Instruction-Based Fast Pseudorandom Number
 * Generator,\n Master's Thesis, Dept. of Math., Hiroshima University
 * (Feb. 2007),\n
 * http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/SFMT/M062821.pdf\n
 * Mutsuo Saito and Makoto Matsumoto,\n
 * SIMD-oriented Fast Mersenne Twister: a 128-bit Pseudorandom Number
 * Generator,\n accepted in the proceedings of MCQMC2006\n
 * http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/ARTICLES/sfmt.pdf
 *
 * Written by Charles Karney
 * <charles@karney.com> and licensed under the LGPL.  For more information, see
 * http://charles.karney.info/random/
 **********************************************************************/

#if !defined(SFMT19937_H)
#define SFMT19937_H

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

#include "RandomLib/RandomSeed.hpp"
#include <limits>
#if HAVE_SSE2
#include <emmintrin.h>
#endif

#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.  0 = scheme implemented in
 * SFMT-src-1.2.tar.gz by M. Saito.
 **********************************************************************/
#define SFMT19937_VERSION 0

namespace RandomLib {
  /**
   * \brief A generator of random bits.
   *
   * This provides an implementation of the SIMD-oriented Fast Mersenne Twister
   * random number generator,
   *
   * SFMT.  See\n Mutsuo Saito,\n An Application of Finite Field: Design
   * and Implementation of 128-bit Instruction-Based Fast Pseudorandom Number
   * Generator,\n Master's Thesis, Dept. of Math., Hiroshima University
   * (Feb. 2007).\n
   * http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/SFMT/M062821.pdf
   * Mutsuo Saito and Makoto Matsumoto,\n
   * SIMD-oriented Fast Mersenne Twister: a 128-bit Pseudorandom Number
   * Generator,\n accepted in the proceedings of MCQMC2006\n
   * http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/ARTICLES/sfmt.pdf
   *
   * The template arguments give the type \a UIntType of the "natural" result
   * and the number of random bits \a BitWidth in each result.  The same
   * underlying SFMT19937 algorithm is used in both cases and this is defined
   * in terms of 128-bit words.  BitWidth controls the size of the words used
   * to implement the algorithm and the size of the result returned by Ran().
   *
   * 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 SFMT19937, \a width is either 32 or 64.  It
   * inherits from RandomSeed the routines for seed management and for
   * converting the seed into state.
   *
   * With the same starting seed, SRandom32::Ran64() and SRandom64::Ran64()
   * produces the same sequences.  Similarly SRandom64::Ran32() produces every
   * other member of the sequence produced by SRandom32::Ran32().
   *
   * This class 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.
   *
   * 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 SFMT19937 : 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 SFMT19937 in units of 128-bit words
       **********************************************************************/
      N128 = (P + 128 - 1)/128,
      /**
       * How many width words per 128-bit word.
       **********************************************************************/
      R = 128 / width,
      /**
       * The long lag for SFMT19937
       **********************************************************************/
      N = N128 * R,
      /**
       * The short lag for SFMT19937  in units of 128-bit words
       **********************************************************************/
      M128 = 122,
      /**
       * The short lag for SFMT19937
       **********************************************************************/
      M = M128 * R
    };
    /**
     * "SMTn" or "SMUn" a signature for Save
     **********************************************************************/
    static const u32 VERSIONID =
      u32(width == 32 ? 0x534d5430UL : 0x534d5530UL) + SFMT19937_VERSION;
    /**
     * Magic matrix for SFMT19937.  Only the low 21 (= 32 - 11) bits need to be
     * set.  (11 is the right shift applied to the words before masking.
     **********************************************************************/
    static const u32 MSK0 = u32(0x1fffefUL),
      MSK1 = u32(0x1ecb7fUL),
      MSK2 = u32(0x1affffUL),
      MSK3 = u32(0x1ffff6UL);
    /**
     * 64-bit versions of MSK.
     **********************************************************************/
    static const u64 LMSK0 = u64(MSK0) | u64(MSK1) << 32,
      LMSK1 = u64(MSK2) | u64(MSK3) << 32;
    /**
     * Mask for simulating u32 << 18 with 64-bit words
     **********************************************************************/
    static const u64 LMSK18 = u64(0xfffc0000fffc0000ULL);
    /**
     * Magic constants needed by "period certification"
     **********************************************************************/
    static const u32 PARITY0 = u32(1UL),
      PARITY1 = u32(0UL),
      PARITY2 = u32(0UL),
      PARITY3 = u32(0x13c9e684UL);
    /**
     * Least significant bit of PARITY
     **********************************************************************/
    static const unsigned PARITY_LSB = 0;
    /**
     * Marker for uninitialized object
     **********************************************************************/
    static const unsigned UNINIT = 0xffffffffU;

    /**
     * \brief Union for SFMT19937's state.
     *
     * A union to hold the state as words of width \a width bits, as 128-bit
     * words (for the SSE2 implemenation) and 32-bit words for seeding.
     **********************************************************************/
    union state {
#if HAVE_SSE2
      /**
       * 128-bit representation.
       **********************************************************************/
      __m128i v[N128];
#endif
      /**
       * 32-bit representation.
       **********************************************************************/
      u32 u[N128 * 4];	// Needed for SeedToState
      /**
       * width-bit representation.
       **********************************************************************/
      result_type w[N];
    };
    /**
     * The state vector
     **********************************************************************/
    state _state;
    /**
     * 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 SFMT19937(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>
    SFMT19937(InputIterator a, InputIterator b)
    { Reseed(a, b); }
    /**
     * Initialize with seed [\a n].  Only the low \e width bits of \a n are
     * used.
     **********************************************************************/
    explicit SFMT19937(seed_type n) throw(std::bad_alloc)
    { Reseed(n); }
    /**
     * Initialize with seed [SeedWord()]
     **********************************************************************/
    SFMT19937() throw(std::bad_alloc) { Reseed(); }
    /**
     * Initialize from a string.  See Reseed(const std::string& s)
     **********************************************************************/
    explicit SFMT19937(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 SFMT19937& 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 SFMT19937& 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.
      SFMT19937 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("SFMT19937: Invalid stride");
      if (k >= n)
	throw std::invalid_argument("SFMT19937: 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::SFMT19937 " << width << "-bit "
	<< "Version " << SFMT19937_VERSION;
      return s.str();
    }

    /**
     * Tests basic engine.  Throws out_of_range errors on bad results.
     **********************************************************************/
    static void SelfTest() {
      SFMT19937 g(std::vector<seed_type>(0));
      g.SetCount(10000-1);
      if (g() !=
	  result_type(width == 32 ? 2695024307ULL : 1464461649847485149ULL))
	throw std::out_of_range("SFMT19937: Incorrect result with seed " +
				g.SeedString());
      seed_type s[] = {0x1234U, 0x5678U, 0x9abcU, 0xdef0U};
      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);
	SFMT19937 h(std::vector<seed_type>(0));
	h.Load(stream, true);
	h.SetCount(1000000-1);
	if (h() !=
	    result_type(width == 32 ? 782200760ULL : 5050640804923595109ULL))
	throw std::out_of_range("SFMT19937: Incorrect result with seed " +
				h.SeedString());
	g.SetCount(1000000);
	if (h != g)
	throw std::out_of_range("SFMT19937: Comparison failure");
      }
    }

  protected:
    /**
     * Return \e width bits of randomness.  This is the natural unit of random
     * data produced by SFMT19937.
     **********************************************************************/
    result_type Ran() throw() {
      if (_ptr >= N)
	Next();
      result_type y = _state.w[_ptr];
      _ptr += _stride;

      return y;
    }

    /**
     * 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 SFMT19937(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.w );
      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 );
      SFMT19937 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.w);
      ar & boost::serialization::make_nvp("checksum", _checksum);
      if (t.Check(_version) != _checksum)
	throw std::out_of_range("SFMT19937: 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 SFMT19937<RandomSeed::u32, 32>::Ran32() throw()
  { return Ran(); }

  template<>
  inline RandomSeed::u64 SFMT19937<RandomSeed::u32, 32>::Ran64() throw()
  // Note that the ordering convention is the opposite of MT19937::Rand64().
  // This is so that Ran64() produces the same sequences with both the 32- and
  // 64-bit versions of SFMT19937.
  { const u64 x = Ran(); return static_cast<u64>(Ran()) << width | x; }

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

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


#include <iostream>
#include <cstdio>
#include <string>
#include <vector>
#include <fstream>
#include <Eigen/Eigenvalues>
#include <opencv2/opencv.hpp>
#include <algorithm>
#include <math.h>

#include "information.h"
#include "functionn.h"
#include "read.h"

using namespace std;
using Eigen::MatrixXd;
using Eigen::EigenSolver;
using Eigen::VectorXcd;
using Eigen::MatrixXcd;

string input,indicator1,indicator2 ;
string dataset = "ATT/";
string dash = "_";
string file_type = ".png";

double all_image_matrix[number_of_dataset][figure_height*figure_width] = { 0 };             //Create a vector*vector to store all image
vector<double> average_face;
vector<double> each_eigenvector(figure_height*figure_width,0);
vector<vector<double>> eigenvectors(number_of_eigenvectors,each_eigenvector);
vector<vector<double>> Wfldupeigenvectors(number_of_eigenvectors, each_eigenvector);
vector<vector<double>> Wflddowneigenvectors(number_of_eigenvectors, each_eigenvector);
double class_average_face[number_of_face_class][figure_height*figure_width] = { 0 };
double class_w[number_of_face_class][number_of_eigenvectors] = { 0 };
int test_index;
int main() {

	image_content image;                                                   //Create a class call "image_content" to store the information of image
	double each[figure_height*figure_width] = {0};                       //Create a vector to store one image

	//Data all_data;

	//Read and store the data from figure and store it into to image_matrix
	for (int i = 0, q = 1, w = 1; i < number_of_dataset && q < 41 && w < 11; i++) {
		indicator1 = to_string(q);
		indicator2 = to_string(w);
		input = dataset + indicator1 + dash + indicator2 + file_type;      //Read the file by substituting the character.
		image = read::figure(input);                                       //Via opencv to read the image and acquire the data in class "image_content" form.
		for (int j = 0; j < figure_height*figure_width; j++) {
			each[j] = (image.content[j]);                                  // Assign the data from class "image_content" to vector variable "each".
		}

		//Store information of each figure to the image_matrix
		for (int k = 0; k < figure_height*figure_width; k++) {
			all_image_matrix[i][k] = each[k];
		}

		//This is just for the pointer.
		if (w < 11) { w++; }
		if (w == 11) { w = 1; q++; }
	}

	average_face = functionn::averagee(all_image_matrix);    //Feed the image_matrix to the function and acquire the average face.

	functionn::save_one_face(average_face);                     //This part is saving the average face to a txt, in order to double check the average face is normal or not.

	//********
	functionn::calculate_St();
	functionn::calculate_Sb();
	functionn::calculate_Sw();
	//return Wopt

	//We got A!  (A = A_matrix)
	//********

	///////////////////////////////////////////////////////////////////////////////
	// Let's calculate eigenvectors!

	//We have two ways to calculate eigenvectors, first is via Matlab, the other is eigen.

	//This part is reading the eigenvector and eigenvalue which are calculated from matlab.
	//Read 6 eigenvalues and its relevant eigenvetors.
	string file_name = "For_matlab/eigenvector";
	string file_type = ".txt";
	ifstream eigenvector_file;
	for (int i = 1; i <= number_of_eigenvectors; i++) {
		int j = 0;
		cout << file_name + to_string(i) + file_type << endl;
		eigenvector_file.open(file_name + to_string(i) + file_type, ios::in);
		string line;
		double tem;
		while (getline(eigenvector_file, line)) {
			tem = stod(line);
			eigenvectors[i - 1][j] = tem;
			j++;
		}
		eigenvector_file.close();
	}

	//Now we have the essiential item to recognize face!
	int per = 0;
	int test_index = 0;
	while (true) {
		//cout << "Please tell me which image do you want to test? (0~49) or enter -1 to exit" << endl;
		//cin >> test_index;
		//cout << endl;

		class_information all;
		//First calculate the aveverge face of each class
		all.calculate_all_average_face();

		//Let's calculate each omega of all classes
		all.calculate_all_w();

		classify result;

		result.calculate_w(all_image_matrix[test_index]);
		result.criteria_each();                          //Calculate the Euclidian distance
		int d_ans=result.determine_result();

		int ans;
		if (test_index < 10) {
			ans = 0;
		}
		else if (test_index < 20) {
			ans = 1;
		}
		else if (test_index < 30) {
			ans = 2;
		}
		else if (test_index < 40) {
			ans = 3;
		}
		else if (test_index < 50) {
			ans = 4;
		}
		else if (test_index < 60) {
			ans = 5;
		}
		else if (test_index < 70) {
			ans = 6;
		}
		else if (test_index < 80) {
			ans = 7;
		}
		else if (test_index < 90) {
			ans = 8;
		}
		else if (test_index < 100) {
			ans = 9;
		}

		//cout << "And the ground truth is " << ans << " !" << endl;

		//cout << endl;

		if (ans == d_ans) {
			per = per + 1;
		}

		if (test_index == number_of_dataset) {
			double acc = double(per) / double(number_of_dataset);
			cout << "Accuracy=" << acc*100 << " %" << " !" << endl;
			break;
		}
		test_index = test_index + 1;
	}
	system("pause");
}


#ifndef CAFFE_UTIL_MKL_ALTERNATE_H_
#define CAFFE_UTIL_MKL_ALTERNATE_H_

#ifdef USE_MKL

#include <mkl.h>

#else  // If use MKL, simply include the MKL header

#ifdef USE_EIGEN

#include <Eigen/Dense>

using Eigen::Map;
using Eigen::VectorXf;
using Eigen::VectorXd;
using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::RowMajor;
using Eigen::InnerStride;

enum CBLAS_ORDER { CblasRowMajor = 101, CblasColMajor = 102 };
enum CBLAS_TRANSPOSE { CblasNoTrans = 111, CblasTrans = 112, CblasConjTrans = 113 };

#define MAP_SVECTOR(name, ptr, N) Map<VectorXf> name(ptr, N)
#define MAP_CONST_SVECTOR(name, ptr, N) Map<const VectorXf> name(ptr, N)
#define MAP_CONST_SVECTOR_STRIDE(name, ptr, N, S) Map<const VectorXf, 0, InnerStride<> > name(ptr, N, InnerStride<>(S))
#define MAP_DVECTOR(name, ptr, N) Map<VectorXd> name(ptr, N)
#define MAP_CONST_DVECTOR(name, ptr, N) Map<const VectorXd> name(ptr, N)
#define MAP_CONST_DVECTOR_STRIDE(name, ptr, N, S) Map<const VectorXd, 0, InnerStride<> > name(ptr, N, InnerStride<>(S))
typedef Matrix<float, Dynamic, Dynamic, RowMajor> MatXf;
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatXd;

#define MAP_SMATRIX(name, ptr, M, N) Map<MatXf> name(ptr, M, N)
#define MAP_CONST_SMATRIX(name, ptr, M, N) Map<const MatXf> name(ptr, M, N)
#define MAP_DMATRIX(name, ptr, M, N) Map<MatXd> name(ptr, M, N)
#define MAP_CONST_DMATRIX(name, ptr, M, N) Map<const MatXd> name(ptr, M, N)

#else

extern "C" {
#include <cblas.h>
}

#endif  // USE_EIGEN

#include <math.h>

// Functions that caffe uses but are not present if MKL is not linked.

// A simple way to define the vsl unary functions. The operation should
// be in the form e.g. y[i] = sqrt(a[i])
#define DEFINE_VSL_UNARY_FUNC(name, operation) \
  template<typename Dtype> \
  void v##name(const int n, const Dtype* a, Dtype* y) { \
    CHECK_GT(n, 0); CHECK(a); CHECK(y); \
    for (int i = 0; i < n; ++i) { operation; } \
  } \
  inline void vs##name( \
    const int n, const float* a, float* y) { \
    v##name<float>(n, a, y); \
  } \
  inline void vd##name( \
      const int n, const double* a, double* y) { \
    v##name<double>(n, a, y); \
  }

DEFINE_VSL_UNARY_FUNC(Sqr, y[i] = a[i] * a[i]);
DEFINE_VSL_UNARY_FUNC(Exp, y[i] = exp(a[i]));
DEFINE_VSL_UNARY_FUNC(Ln, y[i] = log(a[i]));
DEFINE_VSL_UNARY_FUNC(Abs, y[i] = fabs(a[i]));

// A simple way to define the vsl unary functions with singular parameter b.
// The operation should be in the form e.g. y[i] = pow(a[i], b)
#define DEFINE_VSL_UNARY_FUNC_WITH_PARAM(name, operation) \
  template<typename Dtype> \
  void v##name(const int n, const Dtype* a, const Dtype b, Dtype* y) { \
    CHECK_GT(n, 0); CHECK(a); CHECK(y); \
    for (int i = 0; i < n; ++i) { operation; } \
  } \
  inline void vs##name( \
    const int n, const float* a, const float b, float* y) { \
    v##name<float>(n, a, b, y); \
  } \
  inline void vd##name( \
      const int n, const double* a, const float b, double* y) { \
    v##name<double>(n, a, b, y); \
  }

DEFINE_VSL_UNARY_FUNC_WITH_PARAM(Powx, y[i] = pow(a[i], b));

// A simple way to define the vsl binary functions. The operation should
// be in the form e.g. y[i] = a[i] + b[i]
#define DEFINE_VSL_BINARY_FUNC(name, operation) \
  template<typename Dtype> \
  void v##name(const int n, const Dtype* a, const Dtype* b, Dtype* y) { \
    CHECK_GT(n, 0); CHECK(a); CHECK(b); CHECK(y); \
    for (int i = 0; i < n; ++i) { operation; } \
  } \
  inline void vs##name( \
    const int n, const float* a, const float* b, float* y) { \
    v##name<float>(n, a, b, y); \
  } \
  inline void vd##name( \
      const int n, const double* a, const double* b, double* y) { \
    v##name<double>(n, a, b, y); \
  }

DEFINE_VSL_BINARY_FUNC(Add, y[i] = a[i] + b[i]);
DEFINE_VSL_BINARY_FUNC(Sub, y[i] = a[i] - b[i]);
DEFINE_VSL_BINARY_FUNC(Mul, y[i] = a[i] * b[i]);
DEFINE_VSL_BINARY_FUNC(Div, y[i] = a[i] / b[i]);

#ifndef USE_EIGEN

// In addition, MKL comes with an additional function axpby that is not present
// in standard blas. We will simply use a two-step (inefficient, of course) way
// to mimic that.
inline void cblas_saxpby(const int N, const float alpha, const float* X,
                         const int incX, const float beta, float* Y,
                         const int incY) {
  cblas_sscal(N, beta, Y, incY);
  cblas_saxpy(N, alpha, X, incX, Y, incY);
}
inline void cblas_daxpby(const int N, const double alpha, const double* X,
                         const int incX, const double beta, double* Y,
                         const int incY) {
  cblas_dscal(N, beta, Y, incY);
  cblas_daxpy(N, alpha, X, incX, Y, incY);
}

#endif  // USE_EIGEN

#endif  // USE_MKL
#endif  // CAFFE_UTIL_MKL_ALTERNATE_H_


/**
 * @file test_vector_sort.cpp
 * @author Bensuperpc (bensuperpc@gmail.com)
 * @brief
 * @version 1.0.0
 * @date 2021-04-01
 *
 * MIT License
 *
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE vector_sort

#include <algorithm>
#include <boost/predef.h>
#include <random>

#if BOOST_COMP_GNUC
extern "C"
{
#    include "quadmath.h"
}
#endif

#include <boost/multiprecision/cpp_int.hpp>
#if BOOST_COMP_GNUC
#    include <boost/multiprecision/float128.hpp>
#endif
#include <boost/test/unit_test.hpp>
#include "vector/vector_imp.hpp"

BOOST_AUTO_TEST_CASE(test_vector_sort_sort_1)
{
    std::vector<uint64_t> unordered(50);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    // BOOST_FAIL( "Test is not ready yet" );
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_sort<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_sort_2)
{
    std::vector<uint64_t> unordered(200);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_sort<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_qsort_1)
{
    std::vector<uint64_t> unordered(50);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_qsort<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_qsort_2)
{
    std::vector<uint64_t> unordered(200);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_qsort<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_stable_sort_1)
{
    std::vector<uint64_t> unordered(50);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_stable_sort<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_stable_sort_2)
{
    std::vector<uint64_t> unordered(200);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_stable_sort<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_bubble_1)
{
    std::vector<uint64_t> unordered(50);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_bubble<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_bubble_2)
{
    std::vector<uint64_t> unordered(200);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_bubble<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_gnome_1)
{
    std::vector<uint64_t> unordered(50);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_gnome<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_gnome_2)
{
    std::vector<uint64_t> unordered(200);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_gnome<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_insertion_1)
{
    std::vector<uint64_t> unordered(50);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_insertion<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_insertion_2)
{
    std::vector<uint64_t> unordered(200);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_insertion<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_shell_1)
{
    std::vector<uint64_t> unordered(50);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_shell<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}

BOOST_AUTO_TEST_CASE(test_vector_sort_shell_2)
{
    std::vector<uint64_t> unordered(200);
    std::iota(unordered.begin(), unordered.end(), 0);

    auto ordered = unordered;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(unordered), std::end(unordered), rng);
    BOOST_REQUIRE_MESSAGE(ordered != unordered, "ordered == unordered"
                                                    << " instead: "
                                                    << "ordered != unordered");
    my::vector::sort_shell<uint64_t>(unordered);

    BOOST_REQUIRE_EQUAL_COLLECTIONS(unordered.begin(), unordered.end(), ordered.begin(), ordered.end());
}


// Boost.Polygon library voronoi_benchmark.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// 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)

// See http://www.boost.org for updates, documentation, and revision history.

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <numeric>
#include <vector>
#include <utility>

#include <boost/random/mersenne_twister.hpp>
#include <boost/timer.hpp>

typedef boost::int32_t int32;

// Include for the Boost.Polygon Voronoi library.
#include <boost/polygon/voronoi.hpp>
typedef boost::polygon::default_voronoi_builder VB_BOOST;
typedef boost::polygon::voronoi_diagram<double> VD_BOOST;

// Includes for the CGAL library.
#include <CGAL/Quotient.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Segment_Delaunay_graph_filtered_traits_2.h>
#include <CGAL/Segment_Delaunay_graph_2.h>
typedef CGAL::Quotient<CGAL::MP_Float> ENT;
typedef CGAL::Simple_cartesian<double> CK;
typedef CGAL::Simple_cartesian<ENT> EK;
typedef CGAL::Segment_Delaunay_graph_filtered_traits_2<
    CK, CGAL::Field_with_sqrt_tag, EK, CGAL::Field_tag> Gt;
typedef CGAL::Segment_Delaunay_graph_2<Gt> SDT_CGAL;
typedef SDT_CGAL::Point_2 Point_CGAL;

// Includes for the S-Hull library.
#include <s_hull.h>

const int RANDOM_SEED = 27;
const int NUM_TESTS = 6;
const int NUM_POINTS[] = {10, 100, 1000, 10000, 100000, 1000000};
const int NUM_RUNS[] = {100000, 10000, 1000, 100, 10, 1};
std::ofstream bf("benchmark_points.txt",
                 std::ios_base::out | std::ios_base::app);
boost::timer timer;

void format_line(int num_points, int num_tests, double time_per_test) {
  bf << "| " << std::setw(16) << num_points << " ";
  bf << "| " << std::setw(15) << num_tests << " ";
  bf << "| " << std::setw(17) << time_per_test << " ";
  bf << "|" << std::endl;
}

void run_boost_test() {
  boost::mt19937 gen(RANDOM_SEED);
  bf << "Boost.Polygon Voronoi of Points:\n";
  for (int i = 0; i < NUM_TESTS; ++i) {
    timer.restart();
    for (int j = 0; j < NUM_RUNS[i]; ++j) {
      VB_BOOST vb;
      VD_BOOST vd;
      for (int k = 0; k < NUM_POINTS[i]; ++k)
        vb.insert_point(static_cast<int32>(gen()), static_cast<int32>(gen()));
      vb.construct(&vd);
    }
    double time_per_test = timer.elapsed() / NUM_RUNS[i];
    format_line(NUM_POINTS[i], NUM_RUNS[i], time_per_test);
  }
  bf << "\n";
}

void run_cgal_test() {
  boost::mt19937 gen(RANDOM_SEED);
  bf << "CGAL Triangulation of Points:\n";
  for (int i = 0; i < NUM_TESTS; ++i) {
    timer.restart();
    for (int j = 0; j < NUM_RUNS[i]; ++j) {
      SDT_CGAL dt;
      for (int k = 0; k < NUM_POINTS[i]; ++k) {
        dt.insert(Point_CGAL(
            static_cast<int32>(gen()), static_cast<int32>(gen())));
      }
    }
    double time_per_test = timer.elapsed() / NUM_RUNS[i];
    format_line(NUM_POINTS[i], NUM_RUNS[i], time_per_test);
  }
  bf << "\n";
}

void run_shull_test() {
  boost::mt19937 gen(RANDOM_SEED);
  bf << "S-Hull Triangulation of Points:\n";
  // This value is required by S-Hull as it doesn't seem to support properly
  // coordinates with the absolute value higher than 100.
  float koef = 100.0 / (1 << 16) / (1 << 15);
  for (int i = 0; i < NUM_TESTS; ++i) {
    timer.restart();
    for (int j = 0; j < NUM_RUNS[i]; ++j) {
      // S-Hull doesn't deal properly with duplicates so we need
      // to eliminate them before running the algorithm.
      std::vector< pair<int32, int32> > upts;
      std::vector<Shx> pts;
      std::vector<Triad> triads;
      Shx pt;
      for (int k = 0; k < NUM_POINTS[i]; ++k) {
        int32 x = static_cast<int32>(gen());
        int32 y = static_cast<int32>(gen());
        upts.push_back(std::make_pair(x, y));
      }
      // Absolutely the same code is used by the Boost.Polygon Voronoi library.
      std::sort(upts.begin(), upts.end());
      upts.erase(std::unique(upts.begin(), upts.end()), upts.end());
      for (int k = 0; k < upts.size(); ++k) {
        pt.r = koef * upts[k].first;
        pt.c = koef * upts[k].second;
        pt.id = k;
        pts.push_back(pt);
      }
      s_hull_del_ray2(pts, triads);
    }
    double time_per_test = timer.elapsed() / NUM_RUNS[i];
    format_line(NUM_POINTS[i], NUM_RUNS[i], time_per_test);
  }
  bf << "\n";
}

int main() {
  bf << std::setiosflags(std::ios::right | std::ios::fixed)
     << std::setprecision(6);
  run_boost_test();
  run_cgal_test();
  run_shull_test();
  bf.close();
  return 0;
}


// Copyright John Maddock 2008.

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

#ifndef BOOST_MATH_SPECIAL_ROUND_FWD_HPP
#define BOOST_MATH_SPECIAL_ROUND_FWD_HPP

#include <boost/config.hpp>

#ifdef _MSC_VER
#pragma once
#endif

namespace boost
{
   namespace math
   {

   template <class T, class Policy>
   T trunc(const T& v, const Policy& pol);
   template <class T>
   T trunc(const T& v);
   template <class T, class Policy>
   int itrunc(const T& v, const Policy& pol);
   template <class T>
   int itrunc(const T& v);
   template <class T, class Policy>
   long ltrunc(const T& v, const Policy& pol);
   template <class T>
   long ltrunc(const T& v);
#ifdef BOOST_HAS_LONG_LONG
   template <class T, class Policy>
   boost::long_long_type lltrunc(const T& v, const Policy& pol);
   template <class T>
   boost::long_long_type lltrunc(const T& v);
#endif
   template <class T, class Policy>
   T round(const T& v, const Policy& pol);
   template <class T>
   T round(const T& v);
   template <class T, class Policy>
   int iround(const T& v, const Policy& pol);
   template <class T>
   int iround(const T& v);
   template <class T, class Policy>
   long lround(const T& v, const Policy& pol);
   template <class T>
   long lround(const T& v);
#ifdef BOOST_HAS_LONG_LONG
   template <class T, class Policy>
   boost::long_long_type llround(const T& v, const Policy& pol);
   template <class T>
   boost::long_long_type llround(const T& v);
#endif
   template <class T, class Policy>
   T modf(const T& v, T* ipart, const Policy& pol);
   template <class T>
   T modf(const T& v, T* ipart);
   template <class T, class Policy>
   T modf(const T& v, int* ipart, const Policy& pol);
   template <class T>
   T modf(const T& v, int* ipart);
   template <class T, class Policy>
   T modf(const T& v, long* ipart, const Policy& pol);
   template <class T>
   T modf(const T& v, long* ipart);
#ifdef BOOST_HAS_LONG_LONG
   template <class T, class Policy>
   T modf(const T& v, boost::long_long_type* ipart, const Policy& pol);
   template <class T>
   T modf(const T& v, boost::long_long_type* ipart);
#endif

   }
}
#endif // BOOST_MATH_SPECIAL_ROUND_FWD_HPP


//  Copyright (c) 2021 DNV AS
//
//  Distributed under the Boost Software License, Version 1.0.
//  See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt
#include "gtest/gtest.h"

#include "Reflection/Members/GlobalType.h"

#include "Reflection/Objects/Object.h"
#include "Reflection/Reflect.h"
#include "Reflection/Objects/Delegate.h"

#include "Reflection/TypeConversions/BuiltInConversions.h"

#include "Reflection/TypeLibraries/TypeLibraryFactory.h"

#include "Vector3d.h"
#include <string>
#include <boost/mpl/lambda.hpp>
#include <memory>
#include "Reflection/Attributes/ExpressionExpanderAttribute.h"
#include "Reflection/IExpressionEvaluator.h"
#include "boost/lexical_cast.hpp"
#include "FakeExpressionEvaluator.h"

using namespace DNVS::MoFa::Reflection;
using namespace DNVS::MoFa::Reflection::TypeConversions;
using namespace DNVS::MoFa::Reflection::TypeLibraries;
using namespace DNVS::MoFa::Reflection::Objects;

TEST(VectorTest,CrossProduct)
{
	TypeLibraryPointer lib = TypeLibraryFactory::CreateDefaultTypeLibrary();
	AddBuiltInConversions(lib->GetConversionGraph());
	Reflect<Vector3d>(lib);
	Object wrapper(lib,Members::GlobalType());

    Vector3d u(10,0,1);
    Vector3d v(0.5,15.5,-4);
    Vector3d w=u.Cross(v);
    Vector3d x=u*w;

    Object u_reflect(lib,wrapper.Lookup("Vector3d")(10,0,1));
    Object v_reflect(lib,wrapper.Lookup("Vector3d")(0.5,15.5,-4));
    Object w_reflect(u_reflect.Lookup("Cross")(v_reflect));
    Object x_reflect(u_reflect*w_reflect);

    EXPECT_EQ(u,u_reflect.As<const Vector3d&>());
    EXPECT_EQ(v,v_reflect.As<const Vector3d&>());
    EXPECT_EQ(w,w_reflect.As<const Vector3d&>());
    EXPECT_EQ(x,x_reflect.As<const Vector3d&>());

    EXPECT_EQ(Object(lib,u),u_reflect);
    EXPECT_EQ(Object(lib,v),v_reflect);
    EXPECT_EQ(Object(lib,w),w_reflect);
    EXPECT_EQ(Object(lib,x),x_reflect);

    EXPECT_NE(Object(lib,x),v_reflect);
}

TEST(VectorTest, ParseShortSyntax)
{
    TypeLibraryPointer lib = TypeLibraryFactory::CreateDefaultTypeLibrary(false);
    AddBuiltInConversions(lib->GetConversionGraph());
    Reflect<Vector3d>(lib);

    Vector3d baseline(10, 0, 1);

    auto expressionResult = Attributes::ExpandExpression("10 0 1", DNVS::MoFa::Reflection::Types::TypeId<Vector3d>(), FakeExpressionEvaluator(lib));
    EXPECT_TRUE(expressionResult.IsValid());
    EXPECT_EQ("Vector3d(10, 0, 1)", expressionResult.GetString());
}


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 *  @file  testTOAFactor.cpp
 *  @brief Unit tests for "Time of Arrival" factor
 *  @author Frank Dellaert
 *  @author Jay Chakravarty
 *  @date December 2014
 */

#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam_unstable/slam/TOAFactor.h>

#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/format.hpp>

using namespace std;
using namespace gtsam;

// typedefs
typedef Expression<Point3> Point3_;
typedef Expression<Event> Event_;

// units
static const double ms = 1e-3;
static const double cm = 1e-2;

// Create a noise model for the TOA error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));

static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 sensorAt0(0, 0, 0);

//*****************************************************************************
TEST(TOAFactor, NewWay) {
  Key key = 12;
  double measurement = 7;
  TOAFactor factor(key, sensorAt0, measurement, model);
}

//*****************************************************************************
TEST(TOAFactor, WholeEnchilada) {
  // Create sensors
  const double height = 0.5;
  vector<Point3> sensors;
  sensors.push_back(Point3(0, 0, height));
  sensors.push_back(Point3(403 * cm, 0, height));
  sensors.push_back(Point3(403 * cm, 403 * cm, height));
  sensors.push_back(Point3(0, 403 * cm, 2 * height));
  EXPECT_LONGS_EQUAL(4, sensors.size());
  //  sensors.push_back(Point3(200 * cm, 200 * cm, height));

  // Create a ground truth point
  const double timeOfEvent = 0;
  Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);

  // Simulate simulatedTOA
  size_t K = sensors.size();
  vector<double> simulatedTOA(K);
  TimeOfArrival toa;
  for (size_t i = 0; i < K; i++) {
    simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
  }

  // Now, estimate using non-linear optimization
  NonlinearFactorGraph graph;
  Key key = 12;
  for (size_t i = 0; i < K; i++) {
    graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
  }

  // Create initial estimate
  Values initialEstimate;
  // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
  Vector4 delta;
  delta << 0.1, 0.1, -0.1, 0.1;
  Event estimatedEvent = groundTruthEvent.retract(delta);
  initialEstimate.insert(key, estimatedEvent);

  // Optimize using Levenberg-Marquardt optimization.
  LevenbergMarquardtParams params;
  params.setAbsoluteErrorTol(1e-10);
  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
  Values result = optimizer.optimize();

  EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
}
//*****************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//*****************************************************************************


// Copyright © 2013 the Search Authors under the MIT license. See AUTHORS for the list of authors.
#include "../utils/geom2d.hpp"
#include <vector>
#include <cstdio>
#include <boost/optional.hpp>

struct Image;

// A PolyMap is a map that contains a set of polygons.
// It can be used to make visibility queries in a plane
// with respect to ploygonal obstacles.
struct PolyMap {

	typedef boost::optional<geom2d::Poly> Bound;

	PolyMap(FILE*);

	PolyMap(std::vector<geom2d::Poly> ps) : polys(ps) { }

	// Constructs a polygon map from a bitmap of obstacles.
	PolyMap(const bool[], unsigned int, unsigned int);

	// input reads the visibility map from a file.
	void input(FILE*);

	// output writes the visibility map to the given file.
	void output(FILE*) const;

	// draw draws the polygons for the visibility map to
	// the given image.
	void draw(Image&, double lwidth = 1) const;

	// scale scales the visibility map by the
	// given factors in both the x and y directions.
	void scale(double, double);

	// translate translates the visibility map by the
	// give x and y values.
	void translate(double, double);

	// min returns the minimum x and y coordinate
	// in the map;
	geom2d::Pt min() const;

	// max returns the maximum x and y coordinate
	// in the map.
	geom2d::Pt max() const;

	// obstructed returns true if the given point lies
	// within one of the obstacles.
	bool obstructed(const geom2d::Pt &pt) {
		if (bound && !bound->contains(pt))
			return true;
		for (unsigned int i = 0; i < polys.size(); i++) {
			if (polys[i].contains(pt))
				return true;
		}
		return false;
	}

	// isvisible returns true if the two points are visible
	// from eachother.
	//
	// This function assumes that both points are not
	// obstructed.
	bool isvisible(const geom2d::Pt&, const geom2d::Pt&) const;

	std::vector<geom2d::Poly> polys;
	Bound bound;
};


/*
 * CommutingSetGenerator.hpp
 *
 *  Created on: Aug 4, 2017
 *      Author: aqw
 */

#ifndef VQE_TRANSFORMATION_COMMUTINGSETGENERATOR_HPP_
#define VQE_TRANSFORMATION_COMMUTINGSETGENERATOR_HPP_

#include "PauliOperator.hpp"
#include <Eigen/Core>
#include <numeric>

namespace xacc {

namespace vqe {
class CommutingSetGenerator {

private:

	std::pair<Eigen::VectorXi, Eigen::VectorXi> bv(Term& op, int nQubits) {
		Eigen::VectorXi vx = Eigen::VectorXi::Zero(nQubits);
		Eigen::VectorXi vz = Eigen::VectorXi::Zero(nQubits);

		for (auto term : op.ops()) {
			if (term.second == "X") {
				vx(term.first) += 1;
			} else if (term.second == "Z") {
				vz(term.first) += 1;
			} else if (term.second == "Y") {
				vx(term.first) += 1;
				vz(term.first) += 1;
			}
		}

		return std::make_pair(vx, vz);
	};

	int bv_commutator(Term& term1, Term& term2, int nQubits) {
			auto pair1 = bv(term1, nQubits);
			auto pair2 = bv(term2, nQubits);
			auto scalar = pair1.first.dot(pair2.second) + pair1.second.dot(pair2.first);
			return scalar % 2;
		};

public:

	std::vector<std::vector<Term>> getCommutingSet(
			PauliOperator& composite, int n_qubits) {

		std::vector<std::vector<Term>> commuting_ops;
		std::vector<Term> allTerms;
		for (auto& kv : composite.getTerms()) {
			allTerms.push_back(kv.second);
		}

		for (int i = 0; i < allTerms.size(); i++) {

			auto t_i = allTerms[i];

			if (i == 0) {
				commuting_ops.push_back({t_i});
			} else {
				auto comm_ticker = 0;
				for (int j = 0; j < commuting_ops.size(); j++) {
					auto j_op_list = commuting_ops[j];
					int sum = 0;
					int innerCounter = 0;
					for (auto j_op : j_op_list) {
						auto t_jopPtr = allTerms[innerCounter];
						sum += bv_commutator(t_i, t_jopPtr,
								n_qubits);
						innerCounter++;
					}

					if (sum == 0) {
						commuting_ops[j].push_back(t_i);
						comm_ticker += 1;
						break;
					}
				}

				if (comm_ticker == 0) {
					commuting_ops.push_back({t_i});
				}
			}
		}

		return commuting_ops;
	}

};

}
}

#endif


/**
 * @file INSMechanization.hpp
 * @author LauZanMo (LauZanMo@whu.edu.cn)
 * @brief INS mechanization class
 * @version 1.0
 * @date 2021-06-18
 *
 * @copyright Copyright (c) 2021 WHU-Drones
 *
 */
#pragma once

#include <Eigen/Geometry>

#include "DataStorage.hpp"
#include "Utils.hpp"

// using namespace Eigen;

namespace INS {

class INSMechanization {
public:
  INSMechanization(const iNav::NavData& init_nav_data,
                   const iNav::IMUData& init_imu_data);

  ~INSMechanization();

  void SensorUpdate(const iNav::IMUData& data);
  iNav::NavData MechanizationUpdate(bool record_data = true);

protected:
  // IMU output
  Eigen::Vector3d delta_theta_, last_delta_theta_;
  Eigen::Vector3d delta_v_, last_delta_v_;

  // INS mechanization data
  double time_, last_time_, delta_time_;
  Eigen::Vector3d v_proj_n_, last_v_proj_n_, before_last_v_proj_n_;
  Eigen::Quaterniond q_n_to_e_, q_n_last_to_e_last_;
  double h_, h_last_;
  Eigen::Quaterniond q_b_to_n_, q_b_last_to_n_last_;

  Eigen::Vector3d omega_ie_midway_proj_n_;
  Eigen::Vector3d omega_en_midway_proj_n_;

  Eigen::Vector3d v_midway_proj_n_;
  Eigen::Quaterniond q_n_midway_to_e_midway_;
  double h_midway_;
  Eigen::Vector3d g_proj_n_;

  iNav::NavData GetNavState();
  void SetNavState(const iNav::NavData& nav_data);
  void SetNavState(const iNav::NavData& nav_data, const iNav::IMUData imu_data);

private:
  // member function
  void VelocityUpdate();
  void PositionUpdate();
  void AttitudeUpdate();
  void DataRecord();

  Eigen::Vector3d ComputeZeta(const Eigen::Vector3d& v_proj_n,
                              const Eigen::Quaterniond& q_n_to_e,
                              const double& h, const double& delta_t);
  Eigen::Vector3d ComputeZeta(const Eigen::Vector3d& v_proj_n,
                              const Eigen::Quaterniond& q_n_to_e,
                              const double& h, const double& delta_t,
                              Eigen::Vector3d& omega_ie_proj_n,
                              Eigen::Vector3d& omega_en_proj_n);
};

}  // namespace INS


//------------------------------------------------------------------------------
/// \file StateMonad_tests.cpp
/// \author Ernest Yeung
/// \email  ernestyalumni@gmail.com
//------------------------------------------------------------------------------
#include "Categories/Monads/StateMonad.h"

#include <array>
#include <boost/test/unit_test.hpp>
#include <cmath>
#include <type_traits> // std::underlying_type
#include <utility> // std::pair

using Categories::Monads::StateMonad::AsLambdas::bind;
using Categories::Monads::StateMonad::AsLambdas::unit;
using Categories::Monads::StateMonad::Unit;
using Categories::Monads::StateMonad::compose;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Monads)
BOOST_AUTO_TEST_SUITE(StateMonad_tests)

struct TestInputs
{
  double x_;
  bool yes_;
};

enum class TestEnumState: char
{
  a = 'a',
  b = 'b',
  c = 'c'
};

struct TestStateAsEnum
{
  TestEnumState state_;

  explicit TestStateAsEnum(const TestEnumState state):
    state_{state}
  {}

  explicit TestStateAsEnum():
    state_{TestEnumState::a}
  {}
};

struct TestInputs2
{
  double value_;
  double operand_;
};

constexpr std::array<int, 2> two_states {{-1, 1}};

auto test_morphism_f(const TestInputs2& test_inputs)
{
  return [&test_inputs](const int state)
  {
    double resulting_value;
    int resulting_state;
    if (state == two_states[1])
    {
      resulting_value = test_inputs.value_ + test_inputs.operand_;
    }
    else
    {
      resulting_value = test_inputs.value_ - test_inputs.operand_;
    }
    if (resulting_value < 0)
    {
      resulting_state = two_states[1];
    }
    else
    {
      resulting_state = two_states[0];
    }
    return unit(resulting_value)(resulting_state);
  };
}

class TestMorphismf
{
  public:

    TestMorphismf(const TestInputs2& test_inputs):
      inputs_{test_inputs}
    {}

    std::pair<int, double> operator()(const int state)
    {
      double resulting_value;
      int resulting_state;
      if (state == two_states[1])
      {
        resulting_value = inputs_.value_ + inputs_.operand_;
      }
      else
      {
        resulting_value = inputs_.value_ - inputs_.operand_;
      }
      if (resulting_value < 0)
      {
        resulting_state = two_states[1];
      }
      else
      {
        resulting_state = two_states[0];
      }
      return std::pair<int, double>(resulting_state, resulting_value);
    }

    TestInputs2 inputs() const
    {
      return inputs_;
    }

  private:

    TestInputs2 inputs_;
};

auto test_morphism_g(const double y)
{
  return [y](const int state)
  {
    if (y > 0.0)
    {
      if (state == two_states[1])
      {
        return unit(-y)(two_states[0]);
      }
      else
      {
        return unit(y)(two_states[1]);
      }
    }
    else
    {
      return unit(y)(two_states[1]);
    }
  };
}

class TestMorphismg
{
  public:

    TestMorphismg(const double y):
      y_{y}
    {}

    auto operator()(const int state)
    {
      if (y_ > 0.0)
      {
        if (state == two_states[1])
        {
          Unit unit_y {-y_};
          return unit_y(two_states[0]);
        }
        else
        {

          Unit unit_y {y_};
          return unit_y(state);
        }
      }
      else
      {
        Unit unit_y {y_};
        return unit_y(state);
      }
    }

    double y() const
    {
      return y_;
    }

  private:

    double y_;
};

BOOST_AUTO_TEST_SUITE(Unit_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitConstructs)
{
  {
    Unit<int> test_unit {5};
    BOOST_TEST(test_unit.input() == 5);
  }
  {
    int x {5};
    Unit<int> test_unit {x};
    BOOST_TEST(test_unit.input() == 5);
  }
  {
    // error: binding reference of type ‘int&’ to ‘const int’ discards qualifiers
    //constexpr int x {5};
    //Unit<int> test_unit {x};
    //BOOST_TEST(test_unit.input() == 5);
  }
  {
    Unit<TestInputs> test_unit {TestInputs{5.0, true}};
    BOOST_TEST(test_unit.input().x_ == 5.0);
    BOOST_TEST(test_unit.input().yes_ == true);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitReturnsInternalHomFromStateToProductType)
{
  {
    Unit<int> test_unit {5};
    auto result = test_unit(TestEnumState::a);
    BOOST_TEST((result.first == TestEnumState::a));
    BOOST_TEST((result.second == 5));
  }
  {
    const TestInputs test_inputs {5.0, false};
    Unit test_unit {test_inputs};
    auto result = test_unit(TestEnumState::b);
    BOOST_TEST((result.first == TestEnumState::b));
    BOOST_TEST(result.second.x_ == 5.0);
    BOOST_TEST(result.second.yes_ == false);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitAsLambdaReturnsInternalHomFromStateToProductType)
{
  {
    auto result = unit(5)(TestEnumState::a);
    BOOST_TEST((result.first == TestEnumState::a));
    BOOST_TEST(result.second == 5);
  }
  {
    TestInputs test_inputs {42.0, true};
    TestEnumState state {TestEnumState::b};
    auto result = unit(test_inputs)(state);
    BOOST_TEST((result.first == TestEnumState::b));
    BOOST_TEST(result.second.x_ == 42.0);
    BOOST_TEST(result.second.yes_ == true);
  }
}

BOOST_AUTO_TEST_SUITE_END() // Unit_tests

BOOST_AUTO_TEST_SUITE(Morphisms)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TestMorphismsMaptoAnInternalHom)
{
  const TestInputs2 test_inputs {69.0, 42.0};
  int test_state {two_states[0]};
  {
    const auto result = test_morphism_f(test_inputs)(two_states[0]);
    BOOST_TEST(result.first == two_states[0]);
    BOOST_TEST(result.second == 27.0);
  }
  {
    auto morphism_f = TestMorphismf{test_inputs};
    auto result = morphism_f(test_state);
    BOOST_TEST(result.first == two_states[0]);
    BOOST_TEST(result.second == 27.0);
  }
  {
    const auto result = test_morphism_g(42.0)(two_states[1]);
    BOOST_TEST(result.first == two_states[0]);
    BOOST_TEST(result.second == -42.0);
  }
  {
    int state {two_states[1]};
    TestMorphismg g {42.0};
    auto result = g(state);
    //BOOST_TEST(result.first == two_states[0]);
    BOOST_TEST(result.second == -42.0);
  }
}

BOOST_AUTO_TEST_SUITE_END() // Morphisms

BOOST_AUTO_TEST_SUITE(BindTests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BindComposesMorphisms)
{
  {
    auto composed_morphisms = bind(test_morphism_g, test_morphism_f);
    const auto result =
      composed_morphisms(TestInputs2{69.0, 42.0})(two_states[1]);
    BOOST_TEST(result.first == two_states[1]);
    BOOST_TEST(result.second == 111.0);
  }
  {
    auto composed_morphisms = compose(test_morphism_g, test_morphism_f);
    const auto result =
      composed_morphisms(TestInputs2{69.0, 42.0})(two_states[1]);
    BOOST_TEST(result.first == two_states[1]);
    BOOST_TEST(result.second == 111.0);
  }
}

BOOST_AUTO_TEST_SUITE_END() // BindTests

BOOST_AUTO_TEST_SUITE_END() // StateMonad_tests
BOOST_AUTO_TEST_SUITE_END() // Monads
BOOST_AUTO_TEST_SUITE_END() // Categories


#ifndef HYPSYS1D_DG_HANDLER_HPP
#define HYPSYS1D_DG_HANDLER_HPP

#include <memory>
#include <Eigen/Dense>
#include <fmt/format.h>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/polynomial_basis.hpp>

class DGHandler {
  public:

    DGHandler (const std::shared_ptr<Model> &model,
               const PolynomialBasis &poly_basis)
        : poly_basis (poly_basis)
    {
        n_vars = model->get_nvars();
        n_coeff = 1 + poly_basis.get_degree();

        n_quad = 3;
        /// Gauss-Legendre quadrature, degree 5
        quad_points.resize(n_quad);
        quad_points << (1 - sqrt(3./5.))/2., 0.5, (1 + sqrt(3./5.))/2.;

        quad_weights.resize(n_quad);
        quad_weights << 5./18., 4./9., 5./18.;
    }

    /// set quadrature
    void set_quadrature (const Eigen::VectorXd &quad_points_,
                         const Eigen::VectorXd &quad_weights_) const
    {
        quad_points = quad_points_;
        quad_weights = quad_weights_;
    }

    /// get quadrature
    std::pair <Eigen::VectorXd, Eigen::VectorXd> get_quadrature () const
    {
        return {quad_points, quad_weights};
    }

    /// build solution from DG coefficients and the basis
    /// pre-evaluated at a certain point
    Eigen::VectorXd build_sol(const Eigen::VectorXd& u,
                              const Eigen::VectorXd& basis) const;

    /// build solution from DG coefficients at a given reference point
    Eigen::VectorXd build_sol(const Eigen::VectorXd& u,
                              double xi) const;

    /// build cell average
    Eigen::MatrixXd build_cell_avg(const Eigen::MatrixXd& u) const;

    /// return boolean if DG limiting is needed
    bool bool_limit_sol() const {
        if (n_coeff == 1) // no limiting for piecewise constant
            return false;
        return true;
    }

    /// build split solution uSol_m = u0 + um, uSol_p = u0 - up
    /// from DG coefficients
    std::tuple <Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd>
    build_split_sol(const Eigen::MatrixXd& u) const;

    /// build DG coefficients from uSol_m = u0 + um, uSol_p = u0 - up
    void compute_limit_coeffs (Eigen::MatrixXd &u,
                               Eigen::MatrixXd &um,
                               Eigen::MatrixXd &up) const;

  private:
    int n_vars, n_coeff;
    PolynomialBasis poly_basis;

    mutable int n_quad;
    mutable Eigen::VectorXd quad_points, quad_weights;
};

#endif // HYPSYS1D_DG_HANDLER_HPP


/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_PLANE_HPP
#define UTILITIES_GEOMETRY_PLANE_HPP

#include "../UtilitiesAPI.hpp"
#include "../core/Logger.hpp"

#include <boost/optional.hpp>

namespace openstudio{

  // forward declaration
  class Point3d;
  class Vector3d;

  /** Plane defines an infinite plane in 3D space.  The equation of a plane is
   *  a*x + b*y + c*z = d, any point that satisfies this equation is on the plane.
   */
  class UTILITIES_API Plane{
  public:

    /// copy constructor
    Plane(const Plane& other);

    /// construct with point and outward normal, throws openstudio::Exception if outwardNormal has 0 length.
    Plane(const Point3d& point, const Vector3d& outwardNormal);

    /// construct with points, will fit least squares plane.
    /// attempts to align outward normal with that of the points (according to right hand rule).
    /// throws openstudio::Exception if cannot compute plane for these points.
    Plane(const std::vector<Point3d>& points);

    /// virtual destructor
    virtual ~Plane();

    /// get the outward normal of this plane
    Vector3d outwardNormal() const;

    /// is this plane parallel to the other plane
    /// true if absolute value of dot product of outward normals is >= (1-tol)
    bool parallel(const Plane& other, double tol = 0.001) const;

    /// is this plane equal to the other plane
    /// true if dot product of outward normals is >= (1-tol) and abs(d1-d2) <= tol
    bool equal(const Plane& other, double tol = 0.001) const;

    /// is this plane reverse equal to the other plane
    /// true if dot product of outward normals is <= (-1+tol) and abs(d1+d2) <= tol
    bool reverseEqual(const Plane& other, double tol = 0.001) const;

    /// flip the plane so outward normal is opposite
    Plane reversePlane() const;

    /// project a point to this plane
    Point3d project(const Point3d& point) const;

    /// project a vector of points to this plane
    std::vector<Point3d> project(const std::vector<Point3d>& points) const;

    /// returns true if the point is on this plane
    bool pointOnPlane(const Point3d& point, double tol = 0.001) const;

    /// coefficient a
    double a() const;

    /// coefficient b
    double b() const;

    /// coefficient c
    double c() const;

    /// coefficient d
    double d() const;

  private:

    // construct with coefficients
    Plane(double a, double b, double c, double d);

    REGISTER_LOGGER("utilities.Plane");

    double m_a;
    double m_b;
    double m_c;
    double m_d;
  };

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const Plane& plane);

  /// optional Plane
  typedef boost::optional<Plane> OptionalPlane;

  /// vector of Plane
  typedef std::vector<Plane> PlaneVector;

} // openstudio

#endif //UTILITIES_GEOMETRY_BOUNDINGBOX_HPP


#ifndef SPARSE_MATRIX_HPP
#define SPARSE_MATRIX_HPP

#include <vector>
#include <iostream>
#include <iomanip>
#include "mkl_spblas.h"
#include <dfesnippets/Timing.hpp>
#include <dfesnippets/sparse/common.hpp>
#include <dfesnippets/VectorUtils.hpp>
#include <dfesnippets/NumericUtils.hpp>

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

template<typename T = double> using CsrMatrix = boost::numeric::ublas::compressed_matrix<
        T,
        boost::numeric::ublas::row_major,
        0,
        std::vector<unsigned int>
        >;

std::vector<double> SpMV_MKL_ge(char *path,
                std::vector<double> v) {
        int n, nnzs;
        double* values;
        int *col_ind, *row_ptr;

        read_ge_mm_csr(path, &n, &nnzs, &col_ind, &row_ptr, &values);

        std::vector<double> r(n);
        char tr = 'N';
        auto start_time = std::chrono::high_resolution_clock::now();
        mkl_dcsrgemv(&tr, &n, values, row_ptr, col_ind, &v[0], &r[0]);
        auto end_time = std::chrono::high_resolution_clock::now();
        dfesnippets::timing::print_clock_diff("SpMV (CPU)", end_time, start_time);
        dfesnippets::timing::print_spmv_gflops("SpMV (CPU)", nnzs, end_time, start_time);
        return r;
}

std::vector<double> SpMV_MKL_unsym(char *path,
                std::vector<double> v) {
        FILE *f = fopen(path, "r");

        int n, nnzs;
        double* values;
        int *col_ind, *row_ptr;
        read_system_matrix_unsym_csr(f, &n, &nnzs, &col_ind, &row_ptr, &values);
        printf("n: %d, nnzs: %d\n", n, nnzs);

        std::vector<double> r(n);
        char tr = 'N';
        mkl_dcsrgemv(&tr, &n, values, row_ptr, col_ind, &v[0], &r[0]);

        fclose(f);
        return r;
}

std::vector<double> SpMV_MKL_sym(char *path,
                std::vector<double> v) {

        FILE *f = fopen(path, "r");

        int n, nnzs;
        double* values;
        int *col_ind, *row_ptr;
        read_system_matrix_sym_csr(f, &n, &nnzs, &col_ind, &row_ptr, &values);
        printf("n: %d, nnzs: %d\n", n, nnzs);

        std::vector<double> r(n);
        char tr = 'l';
        mkl_dcsrsymv(&tr, &n, values, row_ptr, col_ind, &v[0], &r[0]);

        fclose(f);
        return r;
}

std::vector<double> SpMV_CPU(char *path, std::vector<double> v, bool symmetric) {
  std::vector<double> r2 = SpMV_MKL_unsym(path, v);
  if (symmetric) {
    std::vector<double> r = SpMV_MKL_sym(path, v);
    std::cout << "Checking sym/unsymmetric match...";
    for (size_t i = 0; i < v.size(); i++) {
      if (!dfesnippets::numeric_utils::almost_equal(r[i], r2[i])) {
        printf(" r[%ld] == %f != r[%ld] == %f\n", i, r[i], i, r2[i]);
        exit(1);
      }
    }
    std::cout << "done!" << std::endl;
  }
  return r2;
}

// converts standard CSR to a format where the row_ptr and
// col_ind are merged into one array which contains entries for each
// row, first the length of the row followed by its elements
template<class value_type>
class AdjustedCsrMatrix {

  int empty_rows;

public:
  int n, nnzs;
  std::vector<value_type> values;
  std::vector<int> adjusted_ind;

  AdjustedCsrMatrix(int n_) : n(n_) {
    this->nnzs = 0;
    this->empty_rows = 0;
  }

  void add_row(std::vector<value_type> row_value, std::vector<int> row_col_ind) {
    values.insert(values.end(), row_value.begin(), row_value.end());
    if (row_col_ind.size() == 0) {
      empty_rows++;
    }
    adjusted_ind.push_back(row_col_ind.size());
    adjusted_ind.insert(adjusted_ind.end(), row_col_ind.begin(), row_col_ind.end());
    this->nnzs += row_value.size();
  }

  int get_empty_rows() {
    return empty_rows;
  }

  void print() {
    std::cout << "ACSR Matrix n = " << n << " nnzs = " << nnzs << std::endl;
    std::cout << "  Values (size = " << values.size() << "): ";
    dfesnippets::vectorutils::print_vector(values);
    std::cout << "  Adjusted_indptr (size = " << adjusted_ind.size() << "): ";
    dfesnippets::vectorutils::print_vector(adjusted_ind);
  }

  void print_dense() {
    int pos = 0;
    for (size_t i = 0; i < adjusted_ind.size();) {
      int row_length = adjusted_ind[i++];
      int seen = 0;
      for (int j = 0; j < n; j++) {
        if (seen == row_length) {
          std::cout << " --  ";
          continue;
        }
        if (j == adjusted_ind[i]) {
          i++;
          seen++;
          std::cout << std::setprecision(2) << std::fixed << static_cast<double>(values[pos++]) << " ";
        } else
          std::cout << " --  ";
      }
      std::cout << std::endl;
    }
  }

  std::vector<value_type> dot(std::vector<value_type> x) {
    std::vector<value_type> result(n, 0);

    int pos = 0;
    int row = 0;
    for (size_t i = 0; i < adjusted_ind.size();) {
      int row_length = adjusted_ind[i++];
      int m = row_length + i;
      for (; i < m; i++) {
        result[row] += x[adjusted_ind[i]] * values[pos++];
      }
      row++;
    }

    return result;
  }

  void load_from_csr(double* values, unsigned int* col_ind, unsigned int* row_ptr) {
    std::vector<value_type> row_values;
    std::vector<int> n_col_ind;

    int pos = 0;
    for (int i = 0; i < n; i++) {
      int rowLength = row_ptr[i + 1] - row_ptr[i];
      for (int j = 0; j < rowLength; j++) {
        double value = values[pos];
        row_values.push_back(value);
        n_col_ind.push_back(col_ind[pos]);
        pos++;
      }
      add_row(row_values, n_col_ind);
      row_values.clear();
      n_col_ind.clear();
    }
  }
};

#endif


#include <NTL/lzz_pE.h>

#include <NTL/new.h>

NTL_START_IMPL

zz_pEInfoT::zz_pEInfoT(const zz_pX& NewP)
{
   build(p, NewP);

   _card_base = zz_p::modulus();
   _card_exp = deg(NewP);
}

const ZZ& zz_pE::cardinality()
{
   if (!zz_pEInfo) LogicError("zz_pE::cardinality: undefined modulus");

   do { // NOTE: thread safe lazy init
      Lazy<ZZ>::Builder builder(zz_pEInfo->_card);
      if (!builder()) break;
      UniquePtr<ZZ> p;
      p.make();
      power(*p, zz_pEInfo->_card_base, zz_pEInfo->_card_exp);
      builder.move(p);
   } while (0);

   return *zz_pEInfo->_card;
}

NTL_THREAD_LOCAL SmartPtr<zz_pEInfoT> zz_pEInfo = 0;

void zz_pE::init(const zz_pX& p)
{
   zz_pEContext c(p);
   c.restore();
}

void zz_pEContext::save()
{
   ptr = zz_pEInfo;
}

void zz_pEContext::restore() const
{
   zz_pEInfo = ptr;
}

zz_pEBak::~zz_pEBak()
{
   if (MustRestore) c.restore();
}

void zz_pEBak::save()
{
   c.save();
   MustRestore = true;
}

void zz_pEBak::restore()
{
   c.restore();
   MustRestore = false;
}

const zz_pE& zz_pE::zero()
{
   NTL_THREAD_LOCAL static zz_pE z(INIT_NO_ALLOC);
   return z;
}

istream& operator>>(istream& s, zz_pE& x)
{
   zz_pX y;

   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

void div(zz_pE& x, const zz_pE& a, const zz_pE& b)
{
   zz_pE t;

   inv(t, b);
   mul(x, a, t);
}

void div(zz_pE& x, const zz_pE& a, long b)
{
   NTL_zz_pRegister(B);
   B = b;
   inv(B, B);
   mul(x, a, B);
}

void div(zz_pE& x, const zz_pE& a, const zz_p& b)
{
   NTL_zz_pRegister(B);
   B = b;
   inv(B, B);
   mul(x, a, B);
}

void div(zz_pE& x, long a, const zz_pE& b)
{
   zz_pE t;
   inv(t, b);
   mul(x, a, t);
}

void div(zz_pE& x, const zz_p& a, const zz_pE& b)
{
   zz_pE t;
   inv(t, b);
   mul(x, a, t);
}

void inv(zz_pE& x, const zz_pE& a)
{
   InvMod(x._zz_pE__rep, a._zz_pE__rep, zz_pE::modulus());
}

NTL_END_IMPL


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#include <boost/qvm/mat_operations.hpp>
#include "test_qvm_matrix.hpp"

namespace
    {
    template <int Dim>
    void
    test()
        {
        using namespace boost::qvm;
        test_qvm::matrix<M1,Dim,Dim> m=identity_mat<float,Dim>();
        for( int i=0; i!=Dim; ++i )
            for( int j=0; j!=Dim; ++j )
                BOOST_TEST(m.a[i][j]==float(i==j));
        test_qvm::matrix<M2,Dim,Dim> n(42,1);
        set_identity(n);
        for( int i=0; i!=Dim; ++i )
            for( int j=0; j!=Dim; ++j )
                BOOST_TEST(n.a[i][j]==float(i==j));
        }
    }

int
main()
    {
    test<2>();
    test<3>();
    test<4>();
    test<5>();
    return boost::report_errors();
    }