//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// 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/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_DYNAMIC_COUNTING_BLOOM_FILTER_HPP
#define BOOST_BLOOM_FILTER_DYNAMIC_COUNTING_BLOOM_FILTER_HPP 1

#include <cmath>
#include <vector>

#include <boost/config.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/size.hpp>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_unsigned.hpp>

#include <boost/bloom_filter/detail/counting_apply_hash.hpp>
#include <boost/bloom_filter/hash/default.hpp>

namespace boost {
  namespace bloom_filters {
    template <typename T,
	      size_t BitsPerBin = 4,
	      class HashFunctions = mpl::vector<boost_hash<T> >,
	      typename Block = size_t,
	      typename Allocator = std::allocator<Block> >
    class dynamic_counting_bloom_filter {

      // Block needs to be an integral type
      BOOST_STATIC_ASSERT( boost::is_integral<Block>::value == true);

      // Block needs to be an unsigned type
      BOOST_STATIC_ASSERT( boost::is_unsigned<Block>::value == true);

      // BitsPerBin needs to be greater than 0
      BOOST_STATIC_ASSERT( BitsPerBin > 0);

      // it doesn't make sense to ever support using a BitsPerBin value larger
      // than the number of bits per Block. In that case, the user shouldn't
      // be using a Bloom filter to represent their data.
      BOOST_STATIC_ASSERT( (BitsPerBin < (sizeof(Block) * 8) ) );

      // because of the nature of this implementation, the Bloom filter
      // can have internal fragmentation if the calculation for
      // bins_per_slot has a remainder. The severity of the  internal
      // fragmentation is equal to the remainder * the number of slots.
      // This check prevents internal fragmentation.
      // This also necessarily limits to bin sizes to one of:
      // [1,2,4,8,16,32(64-bit system only)] bits
      BOOST_STATIC_ASSERT( ((sizeof(Block) * 8) % BitsPerBin) == 0);

    public:
      typedef T value_type;
      typedef T key_type;
      typedef HashFunctions hash_function_type;
      typedef Block block_type;
      typedef Allocator allocator_type;
      typedef dynamic_counting_bloom_filter<T, BitsPerBin,
					    HashFunctions,
					    Block, Allocator> this_type;

      typedef std::vector<Block, Allocator> bucket_type;
      typedef typename bucket_type::iterator bucket_iterator;
      typedef typename bucket_type::const_iterator bucket_const_iterator;

      static const size_t slot_bits = sizeof(block_type) * 8;
      static const size_t default_num_bins = 32;

    private:
      size_t bucket_size(const size_t requested_bins) const {
	const size_t bin_bits = requested_bins * BitsPerBin;
	return bin_bits / slot_bits + 1;
      }

      typedef detail::counting_apply_hash<mpl::size<HashFunctions>::value - 1,
					  this_type> apply_hash_type;

    public:
      //* constructors
      dynamic_counting_bloom_filter()
	: bits(bucket_size(default_num_bins)),
	  _num_bins(default_num_bins)
      {
      }

      explicit dynamic_counting_bloom_filter(const size_t requested_bins)
	: bits(bucket_size(requested_bins)),
	  _num_bins(requested_bins)
      {
      }

      template <typename InputIterator>
      dynamic_counting_bloom_filter(const InputIterator start,
				    const InputIterator end)
	: bits(bucket_size(std::distance(start, end) * 4)),
	  _num_bins(std::distance(start, end) * 4)
      {
	for (InputIterator i = start; i != end; ++i)
	  this->insert(*i);
      }

      //* meta functions
      size_t num_bins() const
      {
	return this->_num_bins;
      }

      static BOOST_CONSTEXPR size_t bits_per_bin()
      {
	return BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t bins_per_slot()
      {
	return sizeof(block_type) * 8 / BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t mask()
      {
	return static_cast<Block>(0 - 1) >> (sizeof(Block) * 8 - BitsPerBin);
      }

      size_t bit_capacity() const
      {
        return this->num_bins() * BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t num_hash_functions()
      {
        return mpl::size<HashFunctions>::value;
      }

      double false_positive_rate() const
      {
        const double n = static_cast<double>(this->count());
        static const double k = static_cast<double>(num_hash_functions());
        static const double m = static_cast<double>(this->num_bins());
        static const double e =
	  2.718281828459045235360287471352662497757247093699959574966;
        return std::pow(1 - std::pow(e, -k * n / m), k);
      }

      //? returns the number of bins that have at least 1 bit set
      size_t count() const
      {
	size_t ret = 0;

	for (bucket_const_iterator i = this->bits.begin(),
	       end = this->bits.end();
	     i != end; ++i) {
	  for (size_t bin = 0; bin < this->bins_per_slot(); ++bin) {
	    const size_t offset_bits = bin * BitsPerBin;
	    const size_t target_bits = (*i >> offset_bits) & this->mask();

	    if (target_bits > 0)
	      ++ret;
	  }
	}

        return ret;
      }

      bool empty() const
      {
	return this->count() == 0;
      }

      const bucket_type&
      data() const
      {
	return this->bits;
      }

      //* core ops
      void insert(const T& t)
      {
	apply_hash_type::insert(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void insert(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->insert(*i);
	}
      }

      void remove(const T& t)
      {
	apply_hash_type::remove(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void remove(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->remove(*i);
	}
      }

      bool probably_contains(const T& t) const
      {
	return apply_hash_type::contains(t,
					 this->bits,
					 this->num_bins());
      }

      //* auxiliary ops
      void clear()
      {
	for (bucket_iterator i = bits.begin(), end = bits.end();
	     i != end; ++i)
	  *i = 0;
      }

      void swap(dynamic_counting_bloom_filter& other)
      {
	dynamic_counting_bloom_filter tmp = other;
	other = *this;
	*this = tmp;
      }

      //* equality comparison operators
      template <typename _T, size_t _BitsPerBin,
		typename _HashFns, typename _Block, typename _Allocator>
      friend bool
      operator==(const dynamic_counting_bloom_filter<_T, _BitsPerBin,
						     _HashFns, _Block,
						     _Allocator>& lhs,
		 const dynamic_counting_bloom_filter<_T, _BitsPerBin,
						     _HashFns, _Block,
						     _Allocator>& rhs);

      template <typename _T, size_t _BitsPerBin,
		typename _HashFns, typename _Block, typename _Allocator>
      friend bool
      operator!=(const dynamic_counting_bloom_filter<_T, _BitsPerBin,
						     _HashFns, _Block,
						     _Allocator>& lhs,
		 const dynamic_counting_bloom_filter<_T, _BitsPerBin,
						     _HashFns, _Block,
						     _Allocator>& rhs);

    private:
      bucket_type bits;
      size_t _num_bins;
    };

    template<class T, size_t BitsPerBin, class HashFunctions,
	     typename Block, typename Allocator>
    void
    swap(dynamic_counting_bloom_filter<T, BitsPerBin,
				       HashFunctions, Block,
				       Allocator>& lhs,
	 dynamic_counting_bloom_filter<T, BitsPerBin,
				       HashFunctions, Block,
				       Allocator>& rhs)

    {
      lhs.swap(rhs);
    }

    template<class T, size_t BitsPerBin, class HashFunctions,
	     typename Block, typename Allocator>
    bool
    operator==(const dynamic_counting_bloom_filter<T, BitsPerBin,
						   HashFunctions,
						   Block,
						   Allocator>& lhs,
	       const dynamic_counting_bloom_filter<T, BitsPerBin,
						   HashFunctions,
						   Block,
						   Allocator>& rhs)
    {
      if (lhs.bit_capacity() != rhs.bit_capacity())
	throw detail::incompatible_size_exception();

      return (lhs.bits == rhs.bits);
    }

    template<class T, size_t BitsPerBin, class HashFunctions,
	     typename Block, typename Allocator>
    bool
    operator!=(const dynamic_counting_bloom_filter<T, BitsPerBin,
						   HashFunctions,
						   Block,
						   Allocator>& lhs,
	       const dynamic_counting_bloom_filter<T, BitsPerBin,
						   HashFunctions,
						   Block,
						   Allocator>& rhs)
    {
      if (lhs.bit_capacity() != rhs.bit_capacity())
	throw detail::incompatible_size_exception();

      return !(lhs == rhs);
    }

  } // namespace bloom_filter
} // namespace boost
#endif


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

#include "data.hpp"
#include "../helper.hpp"

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"

using namespace std;

typedef g2o::BlockSolver< g2o::BlockSolverTraits<2, 1> >  KLTBlockSolver;
typedef g2o::LinearSolverCSparse<KLTBlockSolver::PoseMatrixType> KLTLinearSolver;

class VertexKLT : public g2o::BaseVertex<2, Eigen::Vector2d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexKLT()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Vector2d::ConstMapType v(update);
      _estimate += v;
    }
};

class EdgeKLT : public g2o::BaseUnaryEdge<1, Eigen::VectorXd, VertexKLT>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeKLT(const cv::Mat& _img1, const cv::Mat& _Ix, const cv::Mat& _Iy)
    :img1(_img1), Ix(_Ix), Iy(_Iy){}

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    void computeError();
    // void linearizeOplus();

  private:
    const cv::Mat& img1;
    const cv::Mat& Ix;
    const cv::Mat& Iy;
};


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include "SiconosConfig.h"

#pragma GCC diagnostic ignored "-Wunused-local-typedefs"

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//#define BIND_FORTRAN_LOWERCASE_UNDERSCORE
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/lapack.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

namespace lapack = boost::numeric::bindings::lapack;

#include "SiconosVector.hpp"
#include "cholesky.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

#include "NumericsMatrix.h"
#include "NumericsSparseMatrix.h"
#include "CSparseMatrix.h"

//#define DEBUG_MESSAGES
#include "siconos_debug.h"

#ifdef DEBUG_MESSAGES
#include "NumericsVector.h"
#include <cs.h>
#endif

using namespace Siconos;

void SimpleMatrix::PLUFactorizationInPlace()
{
  if (_isPLUFactorized)
  {
    std::cout << "SimpleMatrix::PLUFactorizationInPlace warning: this matrix is already PLUFactorized. " << std::endl;
    return;
  }
  if (_num == Siconos::DENSE)
  {
    if (!_ipiv)
      _ipiv.reset(new VInt(size(0)));
    else
      _ipiv->resize(size(0));
    int info = lapack::getrf(*mat.Dense, *_ipiv);
    if (info != 0)
    {
      _isPLUFactorized = false;
      _isPLUFactorizedInPlace = true;
      THROW_EXCEPTION("SimpleMatrix::PLUFactorizationInPlace failed: the matrix is singular.");
    }
    else
    {
      _isPLUFactorized = true;
      _isPLUFactorizedInPlace = true;
    }
  }
  else
  {
    int info = cholesky_decompose(*sparse());
    // \warning: VA 24/11/2010: work only for symmetric matrices. Should be replaced by efficient implementatation (e.g. mumps )
    if (info != 0)
    {
      display();
      _isPLUFactorized = false;
      _isPLUFactorizedInPlace = false;
      std::cout << "Problem in Cholesky Decomposition for the row number" << info   << std::endl;
      THROW_EXCEPTION("SimpleMatrix::PLUFactorizationInPlace failed. ");
    }
    else
    {
      _isPLUFactorized = true;
      _isPLUFactorizedInPlace = false;
    }
  }

}

void SimpleMatrix::PLUInverseInPlace()
{
  if(!_isPLUFactorized)
    PLUFactorizationInPlace();
  if(_num != Siconos::DENSE)
    THROW_EXCEPTION(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices.");

#if defined(HAS_LAPACK_dgetri)
  int info = lapack::getri(*mat.Dense, *_ipiv);   // solve from factorization

  if(info != 0)
    THROW_EXCEPTION("SimpleMatrix::PLUInverseInPlace failed, the matrix is singular.");

  _isPLUInversed = true;
#else
  THROW_EXCEPTION("SimpleMatrix::PLUInverseInPlace not implemented with lapack.");
#endif
}

void SimpleMatrix::PLUForwardBackwardInPlace(SiconosMatrix &B)
{
  if(B.isBlock())
    THROW_EXCEPTION("SimpleMatrix PLUForwardBackwardInPlace(B) failed at solving Ax = B. Not yet implemented for a BlockMatrix B.");
  int info = 0;

  if(_num == Siconos::DENSE)
  {
    if(!_isPLUFactorized)  // call gesv => LU-factorize+solve
    {
      // solve system:
      if(!_ipiv)
        _ipiv.reset(new VInt(size(0)));
      else
        _ipiv->resize(size(0));
      info = lapack::gesv(*mat.Dense, *_ipiv, *(B.dense()));
      _isPLUFactorized = true;

      /*
        ublas::vector<double> S(std::max(size(0),size(1)));
        ublas::matrix<double, ublas::column_major> U(size(0),size(1));
        ublas::matrix<double, ublas::column_major> VT(size(0),size(1));

        int ierr = lapack::gesdd(*mat.Dense, S, U, VT);
        printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
      */
      // B now contains solution:
    }
    else // call getrs: only solve using previous lu-factorization
      if(B.num() == DENSE)
        info = lapack::getrs(*mat.Dense, *_ipiv, *(B.dense()));
      else
        THROW_EXCEPTION(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices in RHS.");

  }
  else
  {
    if(!_isPLUFactorized)  // call first PLUFactorizationInPlace
    {
      PLUFactorizationInPlace();
    }
    // and then solve
    if(B.num() == Siconos::DENSE)
    {
      inplace_solve(*sparse(), *(B.dense()), ublas::lower_tag());
      inplace_solve(ublas::trans(*sparse()), *(B.dense()), ublas::upper_tag());
    }
    else if(B.num() == Siconos::SPARSE)
    {
      inplace_solve(*sparse(), *(B.sparse()), ublas::lower_tag());
      inplace_solve(ublas::trans(*sparse()), *(B.sparse()), ublas::upper_tag());
    }
    else
      THROW_EXCEPTION(" SimpleMatrix::PLUInverseInPlace: only implemented for dense ans sparse matrices in RHS.");
    info = 0 ;
  }
  //  THROW_EXCEPTION(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices.");

  if(info != 0)
    THROW_EXCEPTION("SimpleMatrix::PLUForwardBackwardInPlace failed.");
}

void SimpleMatrix::PLUForwardBackwardInPlace(SiconosVector &B)
{
  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info;

  if(_num == Siconos::DENSE)
  {
    if(!_isPLUFactorized)  // call gesv => LU-factorize+solve
    {
      // solve system:
      if(!_ipiv)
        _ipiv.reset(new VInt(size(0)));
      else
        _ipiv->resize(size(0));

      info = lapack::gesv(*mat.Dense, *_ipiv, tmpB);
      _isPLUFactorized = true;

      /*
        ublas::matrix<double> COPY(*mat.Dense);
        ublas::vector<double> S(std::max(size(0),size(1)));
        ublas::matrix<double, ublas::column_major> U(size(0),size(1));
`        ublas::matrix<double, ublas::column_major> VT(size(0),size(1));

        int ierr = lapack::gesdd(COPY, S, U, VT);
        printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
      */
      // B now contains solution:
    }
    else // call getrs: only solve using previous lu-factorization
      info = lapack::getrs(*mat.Dense, *_ipiv, tmpB);
  }
  else
  {
    if(!_isPLUFactorized)  // call first PLUFactorizationInPlace
    {
      PLUFactorizationInPlace();
    }
    // and then solve
    inplace_solve(*sparse(), tmpB, ublas::lower_tag());
    inplace_solve(ublas::trans(*sparse()), tmpB, ublas::upper_tag());
    info = 0;
  }
  if(info != 0)
    THROW_EXCEPTION("SimpleMatrix::PLUForwardBackwardInPlace failed.");

  noalias(*(B.dense())) = ublas::column(tmpB, 0);

}

void SimpleMatrix::resetLU()
{
  if(_ipiv) _ipiv->clear();
  _isPLUFactorized = false;
  _isPLUFactorizedInPlace = false;
  _isPLUInversed = false;
}

void SimpleMatrix::resetCholesky()
{
  _isCholeskyFactorized = false;
  _isCholeskyFactorizedInPlace = false;
}

void SimpleMatrix::resetQR()
{
  _isQRFactorized = false;

}

void SimpleMatrix::resetFactorizationFlags()
{
  resetLU();
  resetCholesky();
  resetQR();
}

// const SimpleMatrix operator * (const SimpleMatrix & A, const SimpleMatrix& B )
// {
//   return (DenseMat)prod(*A.dense() , *B.dense());
//   //  return A;
// }

void SimpleMatrix::SolveByLeastSquares(SiconosMatrix &B)
{
  if(B.isBlock())
    THROW_EXCEPTION("SimpleMatrix::SolveByLeastSquares(Siconos Matrix &B) failed. Not yet implemented for M being a BlockMatrix.");
  int info = 0;
#ifdef USE_OPTIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, *(B.dense()), lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, *(B.dense()), lapack::minimal_workspace());
#endif
  if(info != 0)
    THROW_EXCEPTION("SimpleMatrix::SolveByLeastSquares failed.");
}

void SimpleMatrix::SolveByLeastSquares(SiconosVector &B)
{
  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info = 0;

#ifdef USE_OPTIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::minimal_workspace());
#endif
  if(info != 0)
  {
    std::cout << "info = " << info << std::endl;
    THROW_EXCEPTION("SimpleMatrix::SolveByLeastSquares failed.");
  }
  else
  {
    noalias(*(B.dense())) = ublas::column(tmpB, 0);
  }

}

/*
void polePlacement(const SiconosMatrix& A, const SiconosVector& B, SiconosVector& P, bool transpose)
{
  unsigned int n = A.size(0);
  DenseMat AA(n, n);
  DenseMat Q(n, n);
  DenseVect tau(n);
  DenseVect BB(n);
  noalias(AA) = (*A.dense());
  lapack::gehrd(1, n, AA, tau);
  lapack::orghr(n, 1, n, Q, tau);
  noalias(BB) = prod(Q, *B.dense());
}
*/

void SimpleMatrix::Factorize()
{
  DEBUG_BEGIN("void SimpleMatrix::Factorize()\n");
  if(isFactorized())
  {
    std::cout << "SimpleMatrix::PLUFactorize warning: this matrix is already Factorized. " << std::endl;
    return;
  }

  /* set the numericsMatrix */

  updateNumericsMatrix();
  NumericsMatrix * NM = numericsMatrix();

  /* Factorization calling the right method in Numerics */
  int info =1;
  if (isSymmetric())
  {
    if (isPositiveDefinite()) // Cholesky Factorization
    {
      //std::cout << "Cholesky Factorize"<< std::endl;
      info  = NM_Cholesky_factorize(NM);

      if(info != 0)
      {
        _isCholeskyFactorized = false;
        THROW_EXCEPTION("SimpleMatrix::Factorize failed (Cholesky)");
      }
      else
      {
        _isCholeskyFactorized = true;
      }
    }
    else  //  LDLT Factorization
    {
      THROW_EXCEPTION("SimpleMatrix::Factorize failed: LDL^T not yet implemented.");
    }
  }
  else //  LU Factorization  by default
  {
    info  = NM_LU_factorize(NM);

    if(info != 0)
    {
      _isPLUFactorized = false;
      if(_num == DENSE)
        _isPLUFactorizedInPlace = true;
      THROW_EXCEPTION("SimpleMatrix::PLUFactorize failed: the matrix is singular.");
    }
    else
    {
      _isPLUFactorized = true;
       if(_num == DENSE)
         _isPLUFactorizedInPlace = true;
    }
  }
  DEBUG_END("void SimpleMatrix::Factorize()\n");
}

//#define SPARSE_RHS_COPY_TO_DENSE 1

void SimpleMatrix::Solve(SiconosMatrix &B)
{
  if(B.isBlock())
    THROW_EXCEPTION("SimpleMatrix Solve(B) failed at solving Ax = B. Not yet implemented for a BlockMatrix B.");

  int info = 1;
  if(!isFactorized())
  {
    Factorize();
  }
  // and then solve
  NumericsMatrix * NM = _numericsMatrix.get();

#ifdef SPARSE_RHS_COPY_TO_DENSE

  double * b;
  SP::SimpleMatrix Bdense;

  if(B.num() == DENSE)
  {
    b = B.getArray();
  }
  else if(B.num() == SPARSE)
  {
    // First way.
    // We copy to dense since our sparse solver is not able to take
    // into account for sparse r.h.s, yet.
    Bdense.reset (new SimpleMatrix(size(0),size(1)));
    * Bdense= B ;                                                // copy to dense
    b = &(*Bdense->getArray());

    // Second way
    // use inplace_solve of ublas (see above with SolveInPlace)
    // For that, we need to fill our factorization given by NM_LU_factorize
    // into a ublas sparse matrix
    // inplace_solve(*sparse(), *(B.sparse()), ublas::lower_tag());
    // inplace_solve(ublas::trans(*sparse()), *(B.sparse()), ublas::upper_tag());

  }
  else
    THROW_EXCEPTION(" SimpleMatrix::Solve: only implemented for dense and sparse matrices in RHS.");

#else
  B.updateNumericsMatrix();
  NumericsMatrix * NM_B = B.numericsMatrix();
  //NM_display(NM_B);
#endif

  if (isSymmetric())
  {
    if (isPositiveDefinite()) // Cholesky Solving
    {
      //std::cout << "Cholesky Solve"<< std::endl;
#ifdef SPARSE_RHS_COPY_TO_DENSE
      info  = NM_Cholesky_solve(NM, b, B.size(1));
#else
      info  = NM_Cholesky_solve_matrix_rhs(NM, NM_B);
#endif
      if(info != 0)
      {
        THROW_EXCEPTION("SimpleMatrix::Solve failed (Cholesky)");
      }
    }
    else  //  LDLT Factorization
    {
      THROW_EXCEPTION("SimpleMatrix::Solve failed: LDL^T not yet implemented.");
    }
  }
  else //  LU Factorization  by default
  {
#ifdef SPARSE_RHS_COPY_TO_DENSE
    info  = NM_LU_solve(NM, b, B.size(1));
#else
    info  = NM_LU_solve_matrix_rhs(NM, NM_B);
#endif
    if(info != 0)
    {
      THROW_EXCEPTION("SimpleMatrix::PLUFactorize failed: the matrix is singular.");
    }
  }

  if(B.num() == SPARSE)
  {
#ifdef SPARSE_RHS_COPY_TO_DENSE
    B = *Bdense ; // we copy back to sparse.
    //B.displayExpert();
#else
    /* we need to fill back again */
    //B.fromCSC(NM_csc(NM_B));
#endif
  }

  if(info != 0)
    THROW_EXCEPTION("SimpleMatrix::Solve failed.");
}

void SimpleMatrix::Solve(SiconosVector &B)
{
  DEBUG_BEGIN("SimpleMatrix::PLUSolve(SiconosVector &B)\n");

  if(!isFactorized())
  {
    Factorize();
  }

  // and then solve
  int info =1;
  NumericsMatrix * NM;
  double * b;
  SP::SiconosVector Bdense;

  if(B.num() == DENSE)
  {
    NM = _numericsMatrix.get();
    b = B.getArray();
  }
  else if(B.num() == SPARSE)
  {
    // First way. We copy to dense since our sparse solver is not able to take into account sparse r.h.s
    Bdense.reset (new SiconosVector(size(0)));
    * Bdense= B ;
    b = &(*Bdense->getArray());
    NM = _numericsMatrix.get();

    // Second way use inplace_solve of ublas
    // For that, we need to fill our factorization given by NM_LU_factorize into a ublas sparse matrix
    //inplace_solve(*sparse(), *(B.sparse()), ublas::lower_tag());
    //inplace_solve(ublas::trans(*sparse()), *(B.sparse()), ublas::upper_tag());
  }
  else
    THROW_EXCEPTION(" SimpleMatrix::Solve: only implemented for dense and sparse matrices in RHS.");

  if (isSymmetric())
  {
    if (isPositiveDefinite()) // Cholesky Factorization
    {
      //std::cout << "Cholesky Solve"<< std::endl;
      info  = NM_Cholesky_solve(NM, b, 1);

      if(info != 0)
      {
        THROW_EXCEPTION("SimpleMatrix::Solve failed (Cholesky)");
      }
    }
    else  //  LDLT Factorization
    {
      THROW_EXCEPTION("SimpleMatrix::Solve failed: LDL^T not yet implemented.");
    }
  }
  else //  LU Factorization  by default
  {
    info  = NM_LU_solve(NM, b, 1);

    if(info != 0)
    {
      THROW_EXCEPTION("SimpleMatrix::Solve failed (LU)");
    }
  }

  if(B.num() == SPARSE)
  {
    B = *Bdense ;                                                // we copy back to sparse.
  }

  if(info != 0)
    THROW_EXCEPTION("SimpleMatrix::Solve failed.");
  // else
  // {
  //   noalias(*(B.dense())) = ublas::column(tmpB, 0);
  // }
  DEBUG_END("SimpleMatrix::Solve(SiconosVector &B)\n");
}


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "../common.hpp"

using namespace std;

std::ostream nullout(nullptr);

class IntcodeComputer {
    public:
        deque<long> inputs;
        long output = 0; // TODO should this be a list?
        vector<long> backup;
        vector<long> memory;
        bool halted = true;

        void load(istream& is);
        void run();    // Run the computer (until it halts or blocks on input)
        void resume(); // Resumes after being blocked
        bool hasHalted() const { return halted; }; // has the computer halted.

        void dump_memory();

    private:
        std::ostream & debug = nullout; // cout;

        int pc = 0;
        long relative_base = 0;

        long * param(int parameter, bool must_be_address = false);
        int ensure_bounds(int address);

        // Some debug prints
        string param_string(int parameter) const;
        string address_string(int address) const;
};

// Ensure there is enough memory (in the computer) to write to this address.
// This may cause pointers to change! Make sure any existing pointers into memory are refreshed.
int IntcodeComputer::ensure_bounds(int address) {
    assert(address >= 0);
    assert(address < 1024 * 1024 * 1024); // Arbitraray limit
    if (memory.size() < address) {
        memory.resize(address);
    }

    return address;
}

// Returns the the address of a value to read/write.
// This is always a valid pointer.
long * IntcodeComputer::param(int parameter, bool for_writing) {
    int offset = pow(10, parameter + 1);
    int mode = memory.at(pc) / offset % 10;

    if (mode == 0) { // position (read or writing)
        int address = ensure_bounds(memory.at(pc + parameter));
        return &memory[address];
    }
    if (mode == 2) { // relative base (read or writing)
        int address = ensure_bounds(memory.at(pc + parameter) + relative_base);
        return &memory[address];
    }

    assert(!for_writing);

    if (mode == 1) { // immediate (only for reading)
        return &memory[pc + parameter];
    }

    assert(false);
}

string IntcodeComputer::address_string(int address) const {
    if (address < 0 || address >= memory.size()) {
        return " (\?\?\?)";
    }
    return " (" + to_string(memory.at(address)) + ")";
}

// Debug string
string IntcodeComputer::param_string(int parameter) const {
    int offset = pow(10, parameter + 1);
    int mode = memory.at(pc) / offset % 10;
    if (mode == 0) { // position
        int address = memory.at(pc + parameter);
        return "&" + to_string(address) + address_string(address);
    }
    if (mode == 1) { // immediate
        int value = memory.at(pc + parameter);
        return to_string(value);
    }
    if (mode == 2) { // relative base
        int address = memory.at(pc + parameter) + relative_base;
        return "rel &" + to_string(relative_base) + " + " + to_string(memory.at(pc + parameter)) + address_string(address);
    }
    assert(false);
}

void IntcodeComputer::run() {
    this->memory = this->backup; // Reset the software
    ensure_bounds(1024 * 1024);

    this->pc = 0;
    this->relative_base = 0;
    this->halted = false;

    resume();
}

void IntcodeComputer::resume() {
    int last_pc = -1;
    while (pc >= 0 && pc < memory.size()) {
        assert(pc != last_pc); // Stuck in loop
        last_pc = pc;

        int op = memory.at(pc) % 100;
        switch (op) {
            case 1: { // add
                debug << "ADD " << param_string(3) << " = " << param_string(1) << " + " << param_string(2) << endl;

                long * dest = param(3, true);
                long * src1 = param(1);
                long * src2 = param(2);
                *dest = *src1 + *src2;
                pc += 4;
                break;
            }
            case 2: {// mul
                debug << "MUL " << param_string(3) << " = " << param_string(1) << " * " << param_string(2) << endl;

                long * dest = param(3, true);
                long * src1 = param(1);
                long * src2 = param(2);
                *dest = *src1 * *src2;
                pc += 4;
                break;
            }
            case 3: { // input
                debug << "INPUT " << param_string(1) << endl;

                if (inputs.size() <= 0 ) {
                    // Block (a resume() call has to be made)
                    return;
                }

                long * dest = param(1, true);
                *dest = inputs.front();
                inputs.pop_front();

                pc += 2;
                break;
            }
            case 4: { // output
                debug << "OUTPUT " << param_string(1) << endl;
                long * src = param(1);
                output = *src;
                pc += 2;
                break;
            }

            case 5: { // jump-if-true
                debug << "IF " << param_string(1) << " != TRUE then pc = " << param_string(2) << endl;
                long * cond = param(1);
                if (*cond != 0) {
                    pc = *param(2);
                } else {
                    pc += 3;
                }
                break;
            }

            case 6: { // jump-if-false
                debug << "IF " << param_string(1) << " = FALSE then pc = " << param_string(2) << endl;
                long * cond = param(1);
                if (*cond == 0) {
                    pc = *param(2);
                } else {
                    pc += 3;
                }
                break;
            }

            case 7: { // less than
                debug << param_string(3) << " = " << param_string(1) << " < " << param_string(2) << endl;
                long * dest = param(3, true);
                long * src1 = param(1);
                long * src2 = param(2);

                *dest = (*src1 < *src2) ? 1 : 0;

                pc += 4;
                break;
            }

            case 8: { // equals
                debug << param_string(3) << " = " << param_string(1) << " == " << param_string(2) << endl;
                long * dest = param(3, true);
                long * src1 = param(1);
                long * src2 = param(2);

                *dest = (*src1 == *src2) ? 1 : 0;

                pc += 4;
                break;
            }

            case 9: { // adjusts the relative base
                debug << "REL_BASE(" << this->relative_base << ") += " << param_string(1) << endl;

                long * base = param(1);
                this->relative_base += *base;

                pc += 2;
                break;
            }

            case 99: // break
                halted = true;
                return;
            default:
                halted = true;
                cout << "Unhandled op:" << op << " modes:" << (memory.at(pc) / 100) << endl;
                return;
        };
    }

    cout << "No more instructions without halting pc:" << pc << endl;
    dump_memory();
}

void IntcodeComputer::dump_memory() {
    for (int i = 0; i < memory.size(); i++) {
        cout << i << "\t" << memory[i] << endl;
    }
}

void IntcodeComputer::load(istream& in) {
    // Read the program
    this->backup.clear();

    string line;
    while (getline(in, line, ',')) {
        boost::trim(line);

        if (line == "") {
            continue;
        }

        backup.push_back(stoi(line));
    }
}

int main() {
    ifstream file ("2019/9.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    IntcodeComputer computer;
    computer.load(file);
    computer.inputs.push_back(1);
    computer.run();

    long answer1 = computer.output;
    cout << "Answer 9.1: " << answer1 << endl;

    computer.inputs.push_back(2);
    computer.run();

    long answer2 = computer.output;
    cout << "Answer 9.2: " << answer2 << endl;
}


/*
Copyright 2009-2019 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <math/aabb2dpolygon.hpp>
#include <math/polygon_def.hpp>
#include <boost/polygon/detail/iterator_points_to_compact.hpp>
#include <boost/polygon/detail/iterator_compact_to_points.hpp>
#include <core/containers.hpp>
namespace boost
{
  namespace polygon
  {

    template <typename Real>
    struct geometry_concept<eXl::AABB2DPolygon<Real> >{ typedef polygon_90_with_holes_concept /*polygon_90_concept*/ type; };

    template <typename Real>
    struct geometry_concept<eXl::Vector<eXl::Vector2<Real> > >{ typedef polygon_90_concept type; };

    template <typename Real>
    struct geometry_concept<eXl::Vector<eXl::AABB2DPolygon<Real> > >{ typedef polygon_90_set_concept type; };

    template <typename Real>
    struct polygon_90_traits<eXl::AABB2DPolygon<Real> > {
      typedef Real coordinate_type;
      typedef iterator_points_to_compact<typename eXl::Vector<eXl::Vector2<Real> >::const_iterator,eXl::Vector2<Real> > compact_iterator_type;
      typedef eXl::Vector2<Real> point_type;

      static inline compact_iterator_type begin_compact(eXl::AABB2DPolygon<Real> const& t) {
          return compact_iterator_type (t.Border().begin(),t.Border().end());
      }
      static inline compact_iterator_type end_compact(eXl::AABB2DPolygon<Real> const& t) {
          return compact_iterator_type (t.Border().end(),t.Border().end());
      }

      // Get the number of sides of the polygon
      static inline std::size_t size(eXl::AABB2DPolygon<Real> const& t) {
        return t.Border().size();
      }

      // Get the winding direction of the polygon
      static inline winding_direction winding(eXl::AABB2DPolygon<Real> const& t) {
        return unknown_winding;
      }
    };

    template <typename Real>
    struct polygon_90_mutable_traits<eXl::AABB2DPolygon<Real> > {

      template <typename iT>
      static inline eXl::AABB2DPolygon<Real>& set_compact(eXl::AABB2DPolygon<Real> & t,
                                         iT input_begin, iT input_end) {

        eXl::Vector<eXl::Vector2<Real>> tempStor;
        iterator_compact_to_points<iT,eXl::Vector2<Real>> iterBegin(input_begin,input_end);
        iterator_compact_to_points<iT,eXl::Vector2<Real>> iterEnd(input_end,input_end);

        tempStor.assign(iterBegin,iterEnd);

        t = eXl::AABB2DPolygon<Real>(tempStor);
        t.ForceClockwise();
        return t;
      }

    };

    template <typename Real>
    struct polygon_90_traits<eXl::Vector<eXl::Vector2<Real> > > {
      typedef Real coordinate_type;
      typedef iterator_points_to_compact<typename eXl::Vector<eXl::Vector2<Real> >::const_iterator,eXl::Vector2<Real> > compact_iterator_type;
      typedef eXl::Vector2<Real> point_type;

      static inline compact_iterator_type begin_compact(typename eXl::AABB2DPolygon<Real>::PtList const& t) {
          return compact_iterator_type (t.begin(),t.end());
      }
      static inline compact_iterator_type end_compact(typename eXl::AABB2DPolygon<Real>::PtList const& t) {
          return compact_iterator_type (t.end(),t.end());
      }

      // Get the number of sides of the polygon
      static inline std::size_t size(typename eXl::AABB2DPolygon<Real>::PtList const& t) {
        return t.size();
      }

      // Get the winding direction of the polygon
      static inline winding_direction winding(typename eXl::AABB2DPolygon<Real>::PtList const& t) {
        return unknown_winding;
      }
    };

    template <typename Real>
    struct polygon_90_mutable_traits<eXl::Vector<eXl::Vector2<Real> > > {

      template <typename iT>
      static inline typename eXl::AABB2DPolygon<Real>::PtList& set_compact(typename eXl::AABB2DPolygon<Real>::PtList & t,
                                         iT input_begin, iT input_end) {

        iterator_compact_to_points<iT,eXl::Vector2<Real> > iterBegin(input_begin,input_end);
        iterator_compact_to_points<iT,eXl::Vector2<Real> > iterEnd(input_end,input_end);
        t.assign(iterBegin,iterEnd);

        return t;
      }

    };

    template <typename Real,typename enable>
    struct polygon_with_holes_traits<eXl::AABB2DPolygon<Real>,enable> {
         typedef typename eXl::AABB2DPolygon<Real>::PtLists::const_iterator iterator_holes_type;
         typedef typename eXl::AABB2DPolygon<Real>::PtList hole_type;
         static inline iterator_holes_type begin_holes(const eXl::AABB2DPolygon<Real>& t) {
              return t.Holes().begin();
         }
         static inline iterator_holes_type end_holes(const eXl::AABB2DPolygon<Real>& t) {
              return t.Holes().end();
         }
         static inline Real size_holes(const eXl::AABB2DPolygon<Real>& t) {
              return t.Holes().size();
         }
    };

    template <typename Real, typename enable>
    struct polygon_with_holes_mutable_traits<eXl::AABB2DPolygon<Real>,enable> {
         template <typename iT>
         static inline eXl::AABB2DPolygon<Real>& set_holes(eXl::AABB2DPolygon<Real>& t, iT inputBegin, iT inputEnd) {
              //t.Holes().assign(inputBegin, inputEnd);
              for(;inputBegin != inputEnd;++inputBegin)
              {
                typename eXl::AABB2DPolygon<Real>::PtList hole;

                polygon_90_mutable_traits<typename eXl::AABB2DPolygon<Real>::PtList>::set_compact(hole, polygon_90_traits<decltype(*inputBegin)>::begin_compact(*inputBegin),
                  polygon_90_traits<decltype(*inputBegin)>::end_compact(*inputBegin));

                t.HolesRW().push_back(typename eXl::AABB2DPolygon<Real>::PtList());
                t.HolesRW().back().swap(hole);
              }
              t.ForceCClockwiseHoles();
              return t;
         }
    };

    //template <typename T>
    //struct is_polygon_90_set_type<eXl::Vector<T> > {
    //  typedef gtl_yes type;
    //};
    //
    //template <typename T>
    //struct is_mutable_polygon_90_set_type<eXl::Vector<T> > {
    //  typedef gtl_yes type;
    //};

    template <typename Real>
    struct polygon_90_set_traits<eXl::Vector<eXl::AABB2DPolygon<Real>> >
    {
      typedef Real coordinate_type;
      typedef typename eXl::Vector<eXl::AABB2DPolygon<Real>>::const_iterator iterator_type;
      typedef eXl::Vector<eXl::AABB2DPolygon<Real>> operator_arg_type;

      static inline iterator_type begin(const eXl::Vector<eXl::AABB2DPolygon<Real>>& polygon_set)
      {
        return polygon_set.begin();
      }

      static inline iterator_type end(const eXl::Vector<eXl::AABB2DPolygon<Real>>& polygon_set)
      {
        return polygon_set.end();
      }

      static inline orientation_2d orient(const eXl::Vector<eXl::AABB2DPolygon<Real>>&) { return HORIZONTAL; }

      static inline bool clean(const eXl::Vector<eXl::AABB2DPolygon<Real>>&) { return false; }

      static inline bool sorted(const eXl::Vector<eXl::AABB2DPolygon<Real>>&) { return false; }
    };

    template <typename Real>
    struct polygon_90_set_mutable_traits<eXl::Vector<eXl::AABB2DPolygon<Real>> >
    {
      typedef polygon_90_with_holes_concept concept_type;
      template <typename input_iterator_type>
      static inline void set(eXl::Vector<eXl::AABB2DPolygon<Real>>& polygon_set, input_iterator_type input_begin, input_iterator_type input_end, orientation_2d orient)
      {
        polygon_set.clear();
        size_t num_ele = std::distance(input_begin, input_end);
        polygon_set.reserve(num_ele);
        polygon_90_set_data<Real> ps(orient);
        ps.reserve(num_ele);
        ps.insert(input_begin, input_end, orient);
        ps.clean();
        get_90_dispatch(polygon_set, ps, orient, concept_type());
      }
    };

} }

namespace eXl
{
  class Serializer;
  template <typename PolygonType>
  Err Stream_T(PolygonType& iPoly, Serializer iStreamer);

  template <typename Real>
  struct PreciseVector
  {
    typedef eXl::Vector2<typename eXl::PreciseType<Real>::type > type;
  };

  template <typename Real>
  inline typename PreciseVector<Real>::type ToPrecise(Vector2<Real> const& iVec)
  {
    return typename PreciseVector<Real>::type(Math<Real>::ToPrecise(iVec.X()), Math<Real>::ToPrecise(iVec.Y()));
  }

  template <typename Real>
  inline bool VectorNotnullptr(Vector2<Real> const& iVec)
  {
    return iVec.Length() > Math<Real>::ZERO_TOLERANCE;
  }

  template <>
  inline bool VectorNotnullptr<int>(Vector2i const& iVec)
  {
    return iVec != Vector2i::ZERO;
  }

  template <typename Real>
  AABB2DPolygon<Real>::AABB2DPolygon(){}

  template <typename Real>
  void AABB2DPolygon<Real>::Translate(Vector2<Real> const& iTrans)
  {
    for (unsigned int i = 0; i < m_Ext.size(); ++i)
    {
      m_Ext[i] += iTrans;
    }

    for (unsigned int i = 0; i < m_Holes.size(); ++i)
    {
      for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
      {
        m_Holes[i][j] += iTrans;
      }
    }
    m_AABB.m_Data[0] += iTrans;
    m_AABB.m_Data[1] += iTrans;
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Scale(Real iNum, Real iDenom)
  {
    //Polygone valide si edge confondues ??
    for (unsigned int i = 0; i < m_Ext.size(); ++i)
    {
      m_Ext[i] = (m_Ext[i] * iNum) / iDenom;
    }

    for (unsigned int i = 0; i < m_Holes.size(); ++i)
    {
      for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
      {
        m_Holes[i][j] = (m_Holes[i][j] * iNum) / iDenom;
      }
    }

    boost::polygon::extents(m_AABB, *this);
  }

  template <typename Real>
  void AABB2DPolygon<Real>::ScaleComponents(Real iNumX, Real iNumY, Real iDenomX, Real iDenomY)
  {
    for (unsigned int i = 0; i < m_Ext.size(); ++i)
    {
      m_Ext[i].X() = (m_Ext[i].X() * iNumX) / iDenomX;
      m_Ext[i].Y() = (m_Ext[i].Y() * iNumY) / iDenomY;
    }

    for (unsigned int i = 0; i < m_Holes.size(); ++i)
    {
      for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
      {
        m_Holes[i][j].X() = (m_Holes[i][j].X() * iNumX) / iDenomX;
        m_Holes[i][j].Y() = (m_Holes[i][j].Y() * iNumY) / iDenomY;
      }
    }

    boost::polygon::extents(m_AABB, *this);
  }

  template <typename Real>
  AABB2DPolygon<Real>::AABB2DPolygon(Vector<Vector2<Real>> const& iPoints)
  {
    m_Ext.clear();

    if(iPoints.size()>=4)
    {
      m_Ext = iPoints;
      if((m_Ext[0]-m_Ext[1]).GetY() != 0)
      {
        Vector2<Real> tempPt = m_Ext[0];
        //memmove(&m_Points[0],&m_Points[1],m_Points.size() * sizeof(Vector2<Real>));
        m_Ext.erase(m_Ext.begin());
        m_Ext.push_back(tempPt);
      }
    }
    if(m_Ext.front() != m_Ext.back())
      m_Ext.push_back(m_Ext.front());

    boost::polygon::extents(m_AABB,*this);
  }

  template <typename Real>
  AABB2DPolygon<Real>::AABB2DPolygon(AABB2D<Real> const& iBox)
  {
    AddBox(iBox);
    if(m_Ext.front() != m_Ext.back())
      m_Ext.push_back(m_Ext.front());
  }

  template <typename Real>
  void AABB2DPolygon<Real>::RemoveUselessPoints()
  {
    _RemoveUselessPoints(m_Ext);
    for(unsigned int i = 0; i<m_Holes.size(); ++i)
    {
      _RemoveUselessPoints(m_Holes[i]);
    }
  }

  template <class Real>
  void AABB2DPolygon<Real>::_RemoveUselessPoints(Vector<Vector2<Real> >& ioPoints)
  {
    if(ioPoints.size() > 2)
    {
      if(ioPoints.back() == ioPoints.front())
      {
        ioPoints.pop_back();
      }

      for (uint32_t i = 0; i < ioPoints.size() - 1; ++i)
      {
        if (ioPoints[i] == ioPoints[i + 1])
        {
          ioPoints.erase(ioPoints.begin() + i + 1);
        }
      }

      Vector2<Real> prevPt1 = ioPoints[ioPoints.size() - 1];
      Vector2<Real> prevPt2 = ioPoints[ioPoints.size() - 2];
      for (int i = 0; i<(int)ioPoints.size(); ++i)
      {
        unsigned int prevIdx = i > 0 ? i - 1 : (i == -1 ? ioPoints.size() - 2 : ioPoints.size() - 1);
        unsigned int curIdx = i >= 0 ? i : ioPoints.size() - 1;
        Vector2<Real> curPt = ioPoints[curIdx];
        if (curPt != prevPt1 && prevPt1 != prevPt2)
        {
          typename PreciseVector<Real>::type dir1 = ToPrecise<int>(prevPt1 - prevPt2);
          typename PreciseVector<Real>::type dir2 = ToPrecise<int>(curPt - prevPt2);
          typename PreciseType<Real>::type len1 = dir1.Normalize();
          typename PreciseType<Real>::type len2 = dir2.Normalize();
          if (dir1.Dot(dir2) > (1 - eXl::Math<typename PreciseType<Real>::type>::EPSILON) && len1 < len2)
          {
            ioPoints[prevIdx] = curPt;
            ioPoints.erase(ioPoints.begin() + curIdx);
            prevPt1 = curPt;
            --i;
            continue;
          }
        }
        prevPt2 = prevPt1;
        prevPt1 = curPt;
      }

      if(ioPoints.front() != ioPoints.back())
        ioPoints.push_back(ioPoints.front());
    }
    else
    {
      ioPoints.clear();
    }
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Swap(AABB2DPolygon& iOther)
  {
    AABB2D<Real> temp = iOther.m_AABB;
    iOther.m_Ext.swap(m_Ext);
    iOther.m_Holes.swap(m_Holes);
    iOther.m_AABB = m_AABB;
    m_AABB = temp;
  }

  template <typename Real>
  Real AABB2DPolygon<Real>::Perimeter()const
  {
    return boost::polygon::perimeter(*this);
  }

  template <typename Real>
  Real AABB2DPolygon<Real>::Area() const
  {
    return boost::polygon::area(*this);
  }

  template <typename Real>
  void AABB2DPolygon<Real>::ForceClockwise()
  {
    if(boost::polygon::winding(*this) != boost::polygon::CLOCKWISE)
    {
      std::reverse(m_Ext.begin(),m_Ext.end());
    }
  }

  template <typename Real>
  void AABB2DPolygon<Real>::ForceCClockwiseHoles()
  {
    for(unsigned int i = 0;i<m_Holes.size();++i)
    {
      if(boost::polygon::winding(m_Holes[i]) != boost::polygon::COUNTERCLOCKWISE)
      {
        std::reverse(m_Holes[i].begin(),m_Holes[i].end());
      }
    }
  }

  template <typename Real>
  void AABB2DPolygon<Real>::InternalSwap(AABB2DPolygon &iOther)const
  {
    m_Ext.swap(iOther.m_Ext);
    m_Holes.swap(iOther.m_Holes);
    std::swap(iOther.m_AABB, m_AABB);
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Clear()
  {
    m_Ext.clear();
    m_Holes.clear();
    m_AABB = AABB2D<Real>();
  }

  template <typename Real>
  bool AABB2DPolygon<Real>::Empty() const
  {
    return m_Ext.empty();
  }

  template <typename Real>
  void AABB2DPolygon<Real>::GetBoxes(Vector<AABB2D<Real> >& oBoxes)const
  {
    Vector<AABB2DPolygon> meSet;
    meSet.push_back(AABB2DPolygon());
    InternalSwap(meSet[0]);

    Vector<AABB2D<Real>> vBoxes;
    boost::polygon::get_rectangles(vBoxes,meSet);

    Vector<AABB2DPolygon> swappedPoly(1, meSet[0]);
    for(auto& pt : swappedPoly[0].Border())
    {
      std::swap(pt.X(), pt.Y());
    }
    for(auto& hole : swappedPoly[0].Holes())
    {
      for(auto& pt : hole)
      {
        std::swap(pt.X(), pt.Y());
      }
    }

    Vector<AABB2D<Real>> hBoxes;
    boost::polygon::get_rectangles(hBoxes,swappedPoly);

    for(auto const& hBoxSwapped : hBoxes)
    {
      AABB2D<Real> hBox = hBoxSwapped;
      std::swap(hBox.m_Data[0].X(), hBox.m_Data[0].Y());
      std::swap(hBox.m_Data[1].X(), hBox.m_Data[1].Y());
      for(auto const& vBox : vBoxes)
      {
        AABB2D<Real> commonBox;
        commonBox.SetCommonBox(vBox, hBox);
        if(!commonBox.Empty())
        {
          oBoxes.push_back(commonBox);
        }
      }
    }

    InternalSwap(meSet[0]);
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Merge(Vector<AABB2DPolygon>& ioPoly)
  {
    Vector<AABB2DPolygon> res;
    if(ioPoly.size() > 0)
    {
      res.push_back(ioPoly.front());
      for(unsigned int i = 1 ; i<ioPoly.size();++i)
      {
        boost::polygon::operators::operator|=(res,ioPoly[i]);
      }
    }
    ioPoly.swap(res);
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Union(AABB2DPolygon const& iOther, AABB2DPolygon& oPoly)const
  {
    oPoly.Clear();

    Vector<AABB2DPolygon> meSet;
    Vector<AABB2DPolygon> otherSet;

    meSet.push_back(AABB2DPolygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(AABB2DPolygon());
    iOther.InternalSwap(otherSet[0]);

    Vector<AABB2DPolygon> result;
    boost::polygon::assign(result,boost::polygon::operators::operator+(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);
    if(result.size() == 1)
      oPoly.Swap(result[0]);
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Difference(AABB2DPolygon const& iOther, Vector<AABB2DPolygon>& oPoly)const
  {
    oPoly.clear();

    Vector<AABB2DPolygon> meSet;
    Vector<AABB2DPolygon> otherSet;

    meSet.push_back(AABB2DPolygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(AABB2DPolygon());
    iOther.InternalSwap(otherSet[0]);

    boost::polygon::assign(oPoly,boost::polygon::operators::operator-(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);

  }

  template <typename Real>
  void AABB2DPolygon<Real>::Intersection(AABB2DPolygon const& iOther, Vector<AABB2DPolygon>& oPoly)const
  {
    Vector<AABB2DPolygon> meSet;
    Vector<AABB2DPolygon> otherSet;

    meSet.push_back(AABB2DPolygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(AABB2DPolygon());
    iOther.InternalSwap(otherSet[0]);

    boost::polygon::assign(oPoly,boost::polygon::operators::operator&(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Shrink(Real iFactor,Vector<AABB2DPolygon>& oOut)const
  {
    Vector<AABB2DPolygon> meSet;
    meSet.push_back(*this);

    boost::polygon::shrink(meSet,iFactor/2,iFactor/2,iFactor/2,iFactor/2);

    if(meSet.size()>0)
      meSet.swap(oOut);
    else
      oOut.clear();
  }

  template <typename Real>
  void AABB2DPolygon<Real>::Bloat(Real iFactor,AABB2DPolygon& oOut)const
  {
    Vector<AABB2DPolygon> meSet;
    meSet.push_back(*this);

    boost::polygon::bloat(meSet,iFactor/2,iFactor/2,iFactor/2,iFactor/2);

    if(meSet.size()>0)
      meSet[0].Swap(oOut);
    else
      oOut.Clear();
  }

  template <typename Real>
  void AABB2DPolygon<Real>::RemoveTiny(Real iFactor,Vector<AABB2DPolygon>& oOut,Vector<AABB2DPolygon>& oRemoved)const
  {
    oOut.clear();
    oRemoved.clear();
    Vector<AABB2DPolygon> meSet;
    meSet.push_back(*this);

    /*meSet = */boost::polygon::shrink(meSet,iFactor/2,iFactor/2,iFactor/2,iFactor/2);

    /*meSet = */boost::polygon::bloat(meSet,iFactor/2,iFactor/2,iFactor/2,iFactor/2);

    //meSet = boost::polygon::keep(meSet,0,ULLONG_MAX,iFactor,ULLONG_MAX,iFactor,ULLONG_MAX);

    if(meSet.empty())
    {
      oRemoved.push_back(*this);
    }
    else
    {

      Vector<AABB2DPolygon> good;
      good.push_back(AABB2DPolygon());
      InternalSwap(good.back());

      boost::polygon::assign(oRemoved,boost::polygon::operators::operator-(good,meSet));
      InternalSwap(good.back());

      //meSet.back().Swap(*this);
      oOut.swap(meSet);
    }
  }

  template <typename Real>
  int AABB2DPolygon<Real>::AddBox(AABB2D<Real> const& iBox)
  {
    if (Empty())
    {
      m_Ext.push_back(Vector2<Real>(iBox.m_Data[1].X(), iBox.m_Data[0].Y()));
      m_Ext.push_back(Vector2<Real>(iBox.m_Data[0].X(), iBox.m_Data[0].Y()));
      m_Ext.push_back(Vector2<Real>(iBox.m_Data[0].X(), iBox.m_Data[1].Y()));
      m_Ext.push_back(Vector2<Real>(iBox.m_Data[1].X(), iBox.m_Data[1].Y()));

      boost::polygon::extents(m_AABB, *this);

      return 1;
    }
    else
    {

      Vector<Vector2<Real>> temp;
      temp.push_back(Vector2<Real>(iBox.m_Data[1].X(), iBox.m_Data[0].Y()));
      temp.push_back(Vector2<Real>(iBox.m_Data[0].X(), iBox.m_Data[0].Y()));
      temp.push_back(Vector2<Real>(iBox.m_Data[0].X(), iBox.m_Data[1].Y()));
      temp.push_back(Vector2<Real>(iBox.m_Data[1].X(), iBox.m_Data[1].Y()));

      Vector<AABB2DPolygon> meSet;
      Vector<AABB2DPolygon> boxSet;

      meSet.push_back(AABB2DPolygon());
      InternalSwap(meSet[0]);

      boxSet.push_back(AABB2DPolygon());
      boxSet[0].m_Ext.swap(temp);

      boost::polygon::operators::operator|=(boxSet, meSet);
      //meSet.clear();
      //temp.get_polygons(meSet);
      if (boxSet.size() == 1)
      {
        InternalSwap(boxSet[0]);
        boost::polygon::extents(m_AABB, *this);
        return 1;
      }
      else
      {
        InternalSwap(meSet[0]);
        return 0;
      }

      return 1;
    }
  }
}

#include <core/stream/serializer.hpp>

namespace eXl
{

  template <typename PolygonType>
  Err Stream_T(PolygonType& iPoly, Serializer iStreamer)
  {
    iStreamer.BeginStruct();
    iStreamer.PushKey("Border");
    iStreamer &= iPoly.Border();
    iStreamer.PopKey();
    iStreamer.PushKey("Holes");
    iStreamer &= iPoly.Holes();
    iStreamer.PopKey();
    iStreamer.EndStruct();

    RETURN_SUCCESS;
  }

  template <typename Real>
  Err AABB2DPolygon<Real>::Stream(Streamer& iStreamer) const
  {
    Serializer serializer(iStreamer);
    return Stream_T(*this, serializer);
  }

  template <typename Real>
  Err AABB2DPolygon<Real>::Unstream(Unstreamer& iStreamer)
  {
    Serializer serializer(iStreamer);
    Err res = Stream_T(*this, serializer);
    if (res)
    {
      boost::polygon::extents(m_AABB, *this);
    }
    return res;
  }
}


#include "BlocksEngine/pch.h"
#include "BlocksEngine/Core/Math/Vector2.h"

#include <boost/container_hash/hash.hpp>

#include "BlocksEngine/Core/Math/Quaternion.h"

using namespace BlocksEngine;
using namespace DirectX;

//------------------------------------------------------------------------------
// Constructors
//------------------------------------------------------------------------------

template <class T>
Vector2<T>::Vector2() noexcept
    : Vector2Base<T>::Base{0, 0}
{
}

template <class T>
constexpr Vector2<T>::Vector2(const T a) noexcept
    : Vector2Base<T>::Base{a, a}
{
}

template <class T>
Vector2<T>::Vector2(_In_reads_(2) const T* pArray) noexcept
    : Vector2Base<T>::Base{pArray}
{
}

template <class T>
Vector2<T>::Vector2(FXMVECTOR v) noexcept // NOLINT(cppcoreguidelines-pro-type-member-init)
{
    Store(this, v);
}

template <class T>
Vector2<T>::Vector2(const typename Vector2Base<T>::Base& v) noexcept
    : Vector2Base<T>::Base{v.x, v.y}
{
}

template <>
Vector2<float>::Vector2(const Vector2Base<float>::Vector& v) noexcept
    : Vector2Base<float>::Base{v.f[0], v.f[1]}
{
}

template <>
Vector2<int32_t>::Vector2(const Vector2Base<int32_t>::Vector& v) noexcept
    : Vector2Base<int32_t>::Base{v.i[0], v.i[1]}
{
}

template <>
Vector2<uint32_t>::Vector2(const Vector2Base<uint32_t>::Vector& v) noexcept
    : Vector2Base<uint32_t>::Base{v.u[0], v.u[1]}
{
}

//------------------------------------------------------------------------------
// Converter operators
//------------------------------------------------------------------------------

template <class T>
Vector2<T>::operator DirectX::XMVECTOR() const noexcept
{
    return Load(this);
}

//------------------------------------------------------------------------------
// Comparision operators
//------------------------------------------------------------------------------

template <class T>
bool Vector2<T>::operator==(const Vector2<T>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    return XMVector2Equal(v1, v2);
}

template <class T>
bool Vector2<T>::operator!=(const Vector2<T>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    return XMVector2NotEqual(v1, v2);
}

//------------------------------------------------------------------------------
// Assignment operators
//------------------------------------------------------------------------------

template <>
Vector2<float>& Vector2<float>::operator=(const Vector2Base<float>::Vector& f) noexcept
{
    this->x = f.f[0];
    this->y = f.f[1];

    return *this;
}

template <>
Vector2<int32_t>& Vector2<int32_t>::operator=(const Vector2Base<int32_t>::Vector& f) noexcept
{
    this->x = f.i[0];
    this->y = f.i[1];

    return *this;
}

template <>
Vector2<uint32_t>& Vector2<uint32_t>::operator=(const Vector2Base<uint32_t>::Vector& f) noexcept
{
    this->x = f.u[0];
    this->y = f.u[1];

    return *this;
}

template <class T>
template <class U>
Vector2<T>& Vector2<T>::operator+=(const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVectorAdd(v1, v2);
    Store(this, x);

    return *this;
}

template <class T>
template <class U>
Vector2<T>& Vector2<T>::operator-=(const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVectorSubtract(v1, v2);
    Store(this, x);

    return *this;
}

template <class T>
template <class U>
Vector2<T>& Vector2<T>::operator*=(const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVectorMultiply(v1, v2);
    Store(this, x);

    return *this;
}

template <class T>
Vector2<T>& Vector2<T>::operator*=(const float s) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorScale(v1, s);
    Store(this, x);

    return *this;
}

template <class T>
Vector2<T>& Vector2<T>::operator/=(const float s) noexcept
{
    assert(s != 0.0f);

    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorScale(v1, 1.f / s);
    Store(this, x);

    return *this;
}

//------------------------------------------------------------------------------
// Unary operators
//------------------------------------------------------------------------------

template <class T>
Vector2<T> Vector2<T>::operator+() const noexcept
{
    return *this;
}

// TODO: Could be done with XMVectorNegate, but test it
template <>
Vector2<float> Vector2<float>::operator-() const noexcept
{
    return Vector2(-this->x, -this->y);
}

template <>
Vector2<int32_t> Vector2<int32_t>::operator-() const noexcept
{
    return Vector2(-this->x, -this->y);
}

template <>
Vector2<uint32_t> Vector2<uint32_t>::operator-() const noexcept
{
    return Vector2(this->x, this->y);
}

//------------------------------------------------------------------------------
// Hashing
//------------------------------------------------------------------------------

/*template <class T>
std::size_t hash_value(const Vector2<T>& v)
{
    std::size_t seed = 0;
    boost::hash_combine(seed, v.x);
    boost::hash_combine(seed, v.y);

    return seed;
}*/

//------------------------------------------------------------------------------
// Vector operations
//------------------------------------------------------------------------------

template <class T>
template <class U>
bool Vector2<T>::InBounds(const Vector2<U>& bounds) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&bounds);
    return XMVector2InBounds(v1, v2);
}

template <class T>
float Vector2<T>::Length() const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2Length(v1);
    return XMVectorGetX(x);
}

template <class T>
float Vector2<T>::LengthSquared() const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2LengthSq(v1);
    return XMVectorGetX(x);
}

template <class T>
template <class U>
float Vector2<T>::Dot(const Vector2<U>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVector2Dot(v1, v2);
    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
void Vector2<T>::Cross(const Vector2<U>& v, Vector2<V>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR r = XMVector2Cross(v1, v2);
    Store(&result, r);
}

template <class T>
template <class U, class V>
Vector2<U> Vector2<T>::Cross(const Vector2<V>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR r = XMVector2Cross(v1, v2);

    Vector2<U> result;
    Store(&result, r);
    return result;
}

template <class T>
void Vector2<T>::Normalize() noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2Normalize(v1);

    Store(this, x);
}

template <class T>
template <class U>
void Vector2<T>::Normalize(Vector2<U>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2Normalize(v1);
    Store(&result, x);
}

template <class T>
template <class U, class V>
void Vector2<T>::Clamp(const Vector2<U>& vMin, const Vector2<V>& vMax) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&vMin);
    const XMVECTOR v3 = Load(&vMax);
    const XMVECTOR x = XMVectorClamp(v1, v2, v3);

    Store(this, x);
}

template <class T>
template <class U, class V, class W>
void Vector2<T>::Clamp(const Vector2<U>& vMin, const Vector2<V>& vMax, Vector2<W>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&vMin);
    const XMVECTOR v3 = Load(&vMax);
    const XMVECTOR x = XMVectorClamp(v1, v2, v3);

    Store(&result, x);
}

//------------------------------------------------------------------------------
// Static functions
//------------------------------------------------------------------------------

template <class T>
template <class U, class V>
float Vector2<T>::Distance(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR v = XMVectorSubtract(x2, x1);
    const XMVECTOR x = XMVector2Length(v);

    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
float Vector2<T>::DistanceSquared(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR v = XMVectorSubtract(x2, x1);
    const XMVECTOR x = XMVector2LengthSq(v);

    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
void Vector2<T>::Min(const Vector2<U>& v1, const Vector2<V>& v2, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMin(x1, x2);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Min(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMin(x1, x2);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Max(const Vector2<U>& v1, const Vector2<V>& v2, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMax(x1, x2);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Max(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMax(x1, x2);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Lerp(const Vector2<U>& v1, const Vector2<V>& v2, const float t, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Lerp(const Vector2<U>& v1, const Vector2<V>& v2, const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::SmoothStep(const Vector2<U>& v1, const Vector2<V>& v2, float t, Vector2<T>& result) noexcept
{
    t = t > 1.0f ? 1.0f : t < 0.0f ? 0.0f : t; // Clamp value to 0 to 1
    t = t * t * (3.f - 2.f * t);
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::SmoothStep(const Vector2<U>& v1, const Vector2<V>& v2, float t) noexcept
{
    t = t > 1.0f ? 1.0f : t < 0.0f ? 0.0f : t; // Clamp value to 0 to 1
    t = t * t * (3.f - 2.f * t);
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W>
void Vector2<T>::Barycentric(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3, const float f,
                             const float g, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x = XMVectorBaryCentric(x1, x2, x3, f, g);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W>
Vector2<T> Vector2<T>::Barycentric(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3, const float f,
                                   const float g) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x = XMVectorBaryCentric(x1, x2, x3, f, g);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W, class X>
void Vector2<T>::CatmullRom(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3, const Vector2<X>& v4,
                            const float t, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x4 = Load(&v4);
    const XMVECTOR x = XMVectorCatmullRom(x1, x2, x3, x4, t);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W, class X>
Vector2<T> Vector2<T>::CatmullRom(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3,
                                  const Vector2<X>& v4, const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x4 = Load(&v4);
    const XMVECTOR x = XMVectorCatmullRom(x1, x2, x3, x4, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W, class X>
void Vector2<T>::Hermite(const Vector2<U>& v1, const Vector2<V>& t1, const Vector2<W>& v2, const Vector2<X>& t2,
                         const float t, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&t1);
    const XMVECTOR x3 = Load(&v2);
    const XMVECTOR x4 = Load(&t2);
    const XMVECTOR x = XMVectorHermite(x1, x2, x3, x4, t);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W, class X>
Vector2<T> Vector2<T>::Hermite(const Vector2<U>& v1, const Vector2<V>& t1, const Vector2<W>& v2, const Vector2<X>& t2,
                               const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&t1);
    const XMVECTOR x3 = Load(&v2);
    const XMVECTOR x4 = Load(&t2);
    const XMVECTOR x = XMVectorHermite(x1, x2, x3, x4, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Reflect(const Vector2<U>& iVec, const Vector2<V>& nVec, Vector2<T>& result) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Reflect(i, n);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Reflect(const Vector2<U>& iVec, const Vector2<V>& nVec) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Reflect(i, n);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Refract(const Vector2<U>& iVec, const Vector2<V>& nVec, const float refractionIndex,
                         Vector2<T>& result) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Refract(i, n, refractionIndex);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Refract(const Vector2<U>& iVec, const Vector2<V>& nVec, const float refractionIndex) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Refract(i, n, refractionIndex);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
void Vector2<T>::Transform(const Vector2<U>& v, const Quaternion& quat, Vector2<T>& result) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR q = XMLoadFloat4(&quat);
    const XMVECTOR x = XMVector3Rotate(v1, q);

    Store(&result, x);
}

template <class T>
template <class U>
Vector2<T> Vector2<T>::Transform(const Vector2<U>& v, const Quaternion& quat) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR q = XMLoadFloat4(&quat);
    const XMVECTOR x = XMVector3Rotate(v1, q);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
void Vector2<T>::Transform(const Vector2<U>& v, const Matrix& m, Vector2<T>& result) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformCoord(v1, m1);

    Store(&result, x);
}

template <class T>
template <class U>
Vector2<T> Vector2<T>::Transform(const Vector2<U>& v, const Matrix& m) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformCoord(v1, m1);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
_Use_decl_annotations_

void Vector2<T>::Transform(const Vector2<U>* varray, const size_t count, const Matrix& m,
                           Vector2<T>* resultArray) noexcept
{
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    XMVector2TransformCoordStream(resultArray, sizeof Vector2Base<U>::Vector, varray, sizeof Vector2Base<T>::Vector,
                                  count, m1);
}

template <class T>
template <class U>
void Vector2<T>::TransformNormal(const Vector2<U>& v, const Matrix& m, Vector2<T>& result) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformNormal(v1, m1);

    Store(&result, x);
}

template <class T>
template <class U>
Vector2<T> Vector2<T>::TransformNormal(const Vector2<U>& v, const Matrix& m) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformNormal(v1, m1);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
_Use_decl_annotations_

void Vector2<T>::TransformNormal(const Vector2<U>* varray, const size_t count, const Matrix& m,
                                 Vector2<T>* resultArray) noexcept
{
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    XMVector2TransformNormalStream(resultArray, sizeof Vector2Base<U>::Vector, varray, sizeof Vector2Base<T>::Vector,
                                   count, m1);
}

//------------------------------------------------------------------------------
// Binary operators
//------------------------------------------------------------------------------

template <class T, class U, class V>
Vector2<T> operator+(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorAdd(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U, class V>
Vector2<T> operator-(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorSubtract(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U, class V>
Vector2<T> operator*(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMultiply(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U>
Vector2<T> operator*(const Vector2<U>& v, const float s) noexcept
{
    const XMVECTOR x1 = Load(&v);
    const XMVECTOR x = XMVectorScale(x1, s);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U, class V>
Vector2<T> operator/(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorDivide(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U>
Vector2<T> operator/(const Vector2<U>& v, const float s) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR x = XMVectorScale(v1, 1.f / s);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U>
Vector2<T> operator*(const float s, const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR x = XMVectorScale(v1, s);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template struct Vector2<float>;
template struct Vector2<int32_t>;
template struct Vector2<uint32_t>;


#include "geometric/bounding_box.hpp"

#include <algorithm>
#include <cmath>       // for atan, exp
#include <type_traits> // for move, swap

#include <boost/math/constants/constants.hpp>

namespace nepomuk
{
namespace geometric
{

WGS84BoundingBox::WGS84BoundingBox(WGS84Coordinate lower_left, WGS84Coordinate upper_right)
    : lower_left(std::move(lower_left)), upper_right(std::move(upper_right))
{
}

WGS84BoundingBox::WGS84BoundingBox(std::uint32_t const horizontal,
                                   std::uint32_t const vertical,
                                   std::uint32_t const zoom_level,
                                   double const tile_size,
                                   std::int32_t const mercator_buffer)
{
    double min_lon = horizontal * tile_size - mercator_buffer;
    double max_lon = (horizontal + 1.0) * tile_size + mercator_buffer;

    double min_lat = (vertical + 1.0) * tile_size + mercator_buffer;
    double max_lat = vertical * tile_size - mercator_buffer;

    // 2^z * TILE_SIZE
    const double shift = (1u << static_cast<unsigned>(zoom_level)) * tile_size;

    auto const shift_lon = [shift](double const lon) {
        return (lon - 0.5 * shift) / shift * 360.0;
    };

    auto const shift_lat = [shift](double const lat) {
        auto const tmp =
            boost::math::constants::pi<double>() * constants::rad_to_degree * (1 - 2 * lat / shift);

        auto const clamped = std::max<long double>(-180., std::min<long double>(180., tmp));
        auto const normalized =
            constants::rad_to_degree * 2. * std::atan(std::exp(clamped * constants::degree_to_rad));

        return normalized - 90.;
    };

    min_lon = shift_lon(min_lon);
    max_lon = shift_lon(max_lon);
    min_lat = shift_lat(min_lat);
    max_lat = shift_lat(max_lat);

    lower_left = WGS84Coordinate(makeLatLonFromDouble<FixedLongitude>(min_lon),
                                 makeLatLonFromDouble<FixedLatitude>(min_lat));
    upper_right = WGS84Coordinate(makeLatLonFromDouble<FixedLongitude>(max_lon),
                                  makeLatLonFromDouble<FixedLatitude>(max_lat));
}

bool WGS84BoundingBox::contains(WGS84Coordinate const &coordinate) const
{
    return lower_left.longitude <= coordinate.longitude &&
           coordinate.longitude <= upper_right.longitude &&
           lower_left.latitude <= coordinate.latitude &&
           coordinate.latitude <= upper_right.latitude;
}

double WGS84BoundingBox::width() const
{
    return doubleFromLatLon(upper_right.longitude) - doubleFromLatLon(lower_left.longitude);
}

double WGS84BoundingBox::height() const
{
    return doubleFromLatLon(upper_right.latitude) - doubleFromLatLon(lower_left.latitude);
}

MercatorBoundingBox::MercatorBoundingBox(MercatorCoordinate lower_left_,
                                         MercatorCoordinate upper_right_)
    : lower_left(lower_left_), upper_right(upper_right_)
{
    // mercator transformations switch invert latitude (counting from upper right). If the
    // coordinates don't match up (e.g. due to conversion issues) we need to swap them to the
    // correct order
    if (lower_left.latitude > upper_right.latitude)
        std::swap(lower_left.latitude, upper_right.latitude);
}

MercatorBoundingBox::MercatorBoundingBox(std::uint32_t const horizontal,
                                         std::uint32_t const vertical,
                                         std::uint32_t const zoom_level,
                                         double const tile_size,
                                         std::int32_t const mercator_buffer)
{
    WGS84BoundingBox bbox(horizontal, vertical, zoom_level, tile_size, mercator_buffer);
    lower_left = MercatorCoordinate(bbox.lower_left);
    upper_right = MercatorCoordinate(bbox.upper_right);
    std::swap(lower_left.latitude, upper_right.latitude);
}

bool MercatorBoundingBox::contains(MercatorCoordinate const &coordinate) const
{
    return lower_left.longitude <= coordinate.longitude &&
           coordinate.longitude <= upper_right.longitude &&
           lower_left.latitude <= coordinate.latitude &&
           coordinate.latitude <= upper_right.latitude;
}

double MercatorBoundingBox::width() const
{
    return doubleFromLatLon(upper_right.longitude) - doubleFromLatLon(lower_left.longitude);
}
double MercatorBoundingBox::height() const
{
    return doubleFromLatLon(upper_right.latitude) - doubleFromLatLon(lower_left.latitude);
}

} // namespace geometric
} // namespace nepomuk


#include <iostream>
#include <boost/astronomy/coordinate/cartesian_representation.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/io.hpp>

using namespace boost::astronomy::coordinate;
using namespace boost::units;
using namespace boost::units::si;

typedef boost::geometry::model::point<double, 3, boost::geometry::cs::spherical<boost::geometry::degree>> geometry_point;

int main()
{
    //creating point without any value
    cartesian_representation<double, quantity<si::length>, quantity<si::length>, quantity<si::length>> point1;

    //creating a point by directly providing values of x, y and z
    auto point2 = make_cartesian_representation(10.0 * meter, 20.0 * meter, 30.0 * meter);

    //assigning value of individual component
    point1.set_x(50.0 * meter);
    point1.set_y(40.0 * meter);
    point1.set_z(880.0 * meter);

    //or to set all the values in single statement tuple could be used as follow
    point2.set_x_y_z(38.5 * meter, 50.0 * meter, 64.23 * meter);

    //creating a point from another point
    auto point3 = make_cartesian_representation(point2);

    //creating a point from boost::geometry::model::point
    //any type of point can be used here
    //here we will demonstrate with cartesian point
    //https://www.boost.org/doc/libs/1_71_0/libs/geometry/doc/html/geometry/reference/models/model_point.html
    //https://www.boost.org/doc/libs/1_71_0/libs/geometry/doc/html/geometry/reference/cs.html
    geometry_point gp(45.0, 60.0, 50.0);
    auto point4 = make_cartesian_representation<double, quantity<si::length>, quantity<si::length>, quantity<si::length>>(gp);

    //accessing each component of representation
    std::cout << point4.get_x() << point4.get_y()<<std::endl; //methods get_y and get_z are available

    //get boost::geometry::model::point of current object
    auto stored_point = point3.get_point();

    //get the tuple of the component in the coordinate
    std::tuple<quantity<si::length>, quantity<si::length>, quantity<si::length>>
        components = point3.get_x_y_z();

    std::cin.get();
    return 0;
}


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

/*
 Copyright (C) 2008 Lorella Fatone
 Copyright (C) 2008 Francesca Mariani
 Copyright (C) 2008 Maria Cristina Recchioni
 Copyright (C) 2008 Francesco Zirilli
 Copyright (C) 2008 StatPro Italia srl

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

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

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

#include <ql/experimental/varianceoption/integralhestonvarianceoptionengine.hpp>
#include <ql/errors.hpp>
#include <ql/function.hpp>
#include <boost/scoped_array.hpp>
#include <complex>

namespace QuantLib {

    namespace {

    /*
     *****************************************************************
     **
     ** Parameters defining the initial condition of the Heston model
     ** and the European call option
     **
     *****************************************************************
     */
    /*
     *****************************************************************
     ** Assign: v0, eprice, tau, rtax
     ******************************************************************
     ******************************************************************
     **     v0: initial variance
     ** eprice: realized variance strike price
     **    tau: time to maturity
     *    rtax: risk free interest rate
     ****************************************************************
     */

    typedef std::complex<Real> Complex;

    Real IvopOneDim(Real eps, Real chi, Real theta, Real /*rho*/,
                      Real v0, Real eprice, Time tau, Real rtax)
    {
        Real ss=0.0;
        boost::scoped_array<double> xiv(new double[2048*2048+1]);
        double nris=0.0;
        int j=0,mm=0;
        double pi=0,pi2=0;
        double dstep=0;
        Real option=0, impart=0;

        boost::scoped_array<Complex> ff(new Complex[2048*2048]);
        Complex xi;
        Complex ui,beta,zita,gamma,csum,vero;
        Complex contrib, caux, caux1,caux2,caux3;

        ui=Complex(0.0,1.0);

        /*
         **********************************************************
         **   i0: initial integrated variance i0=0
         **********************************************************
         */
        Real i0=0.0;
        //s=2.0*chi*theta/(eps*eps)-1.0;

        //s=s+1;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************
         */

        pi= 3.14159265358979324;
        pi2=2.0*pi;
        Real s=2.0*chi*theta/(eps*eps)-1.0;
        /*
         ****************************************
         ** Note that s must be greater than zero
         ****************************************
         */

        if(s<=0)
        {
            QL_FAIL("this parameter must be greater than zero-> " << s);
        }

        ss=s+1;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************

         **************************************************************
         ** The oscillatory integral that approximates the price of
         ** the realized variance option is computed using the method
         ** proposed by Bailey, Swarztrauber in the paper published in
         ** Siam Journal on Scientific Computing Vol 15(5) 1994
         ** p. 1105-1110
         **************************************************************

         **************************************************************
         ** dstep: real number, generally a power of two, that must be
         **        assigned to determine the grid of
         **        integration. Hint: dstep=256 or 512 (dstep<=2048)
         **************************************************************
         */
        dstep=256.0;
        nris=std::sqrt(pi2)/dstep;
        mm=(int)(pi2/(nris*nris));

        /*
         ******************************************
         **  Definition of the integration grid  **
         ******************************************
         */
        for (j=0;j<=mm-1;j++)
        {
            xiv[j+1]=(double)(j-mm/2)*nris;
        }

        for (j=0;j<=mm-1;j++)
        {
            xi=xiv[j+1];
            caux=chi*chi;
            caux1=2.0*eps*eps;
            caux1=caux1*xi;
            caux1=caux1*ui;
            caux2=caux1+caux;

            zita=0.5*std::sqrt(caux2);

            caux1=std::exp(-2.0*tau*zita);

            beta=0.5*chi+zita;
            beta=beta+caux1*(zita-0.5*chi);
            gamma=1.0-caux1;

            caux=-ss*tau;
            caux2=caux*(zita-0.5*chi);
            caux=ss*std::log(2.0*(zita/beta));
            caux3=-v0*ui*xi*(gamma/beta);
            caux=caux+caux3;
            caux=caux+caux2;

            ff[j+1]=std::exp(caux);
            if(std::sqrt(std::imag(xi)*std::imag(xi)+std::real(xi)*std::real(xi))>1.e-06)
            {
                contrib=-eprice/(ui*xi);
                caux=ui*xi;
                caux=caux*eprice;
                caux=std::exp(caux);
                caux=caux-1.0;
                caux2=ui*xi*ui*xi;
                contrib=contrib+caux/caux2;
            }
            else
            {
                contrib=eprice*eprice*0.5;
            }
            ff[j+1]=ff[j+1]*contrib;
        }
        csum=0.0;
        for (j=0;j<=mm-1;j++)
        {
            caux=std::pow(-1.0,j);
            caux2=-2.0*pi*(double)mm*(double)j*0.5/(double)mm;
            caux3=ui*caux2;
            csum=csum+ff[j+1]*caux*std::exp(caux3);
        }
        csum=csum*std::sqrt(std::pow(-1.0,mm))*nris/pi2;
        vero=i0-eprice+theta*tau+(1.0-std::exp(-chi*tau))*(v0-theta)/chi;
        csum=csum+vero;
        option=std::exp(-rtax*tau)*std::real(csum);
        impart=std::imag(csum);
        QL_ENSURE(impart <= 1e-12,
                  "imaginary part option (must be zero) = " << impart);
        return option;
    }

    Real IvopTwoDim(Real eps, Real chi, Real theta, Real /*rho*/,
                    Real v0, Time tau, Real rtax,
                    const ext::function<Real(Real)>& payoff) {

        Real ss=0.0;
        boost::scoped_array<double> xiv(new double[2048*2048+1]);
        boost::scoped_array<double> ivet(new double[2048 * 2048 + 1]);
        double nris=0.0;
        int j=0,mm=0,k=0;
        double pi=0,pi2=0;

        double dstep=0;
        Real ip=0;
        Real payoffval=0;
        Real option=0/*, impart=0*/;

        Real sumr=0;//,sumi=0;
        Complex dxi,z;

        boost::scoped_array<Complex> ff(new Complex[2048*2048]);
        Complex xi;
        Complex ui,beta,zita,gamma,csum;
        Complex caux,caux1,caux2,caux3;

        ui=Complex(0.0,1.0);

        /*
         **********************************************************
         **   i0: initial integrated variance i0=0
         **********************************************************
         */
        Real i0=0.0;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************
         */

        pi= 3.14159265358979324;
        pi2=2.0*pi;

        Real s=2.0*chi*theta/(eps*eps)-1.0;
        /*
         ****************************************
         ** Note that s must be greater than zero
         ****************************************
         */

        if(s<=0)
        {
            QL_FAIL("this parameter must be greater than zero-> " << s);
        }

        ss=s+1;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************

         **************************************************************
         ** The oscillatory integral that approximates the price of
         ** the realized variance option is computed using the method
         ** proposed by Bailey, Swarztrauber in the paper published in
         ** Siam Journal on Scientific Computing Vol 15(5) 1994
         ** p. 1105-1110
         **************************************************************

         **************************************************************
         ** dstep: real number, generally a power of two that must be
         **        assigned to determine the grid of
         **        integration. Hint: dstep=256 or 512 (dstep<=2048)
         **************************************************************
         */
        dstep=64.0;
        nris=std::sqrt(pi2)/dstep;
        mm=(int)(pi2/(nris*nris));

        /*
         ******************************************
         **  Definition of the integration grid  **
         ******************************************
         */

        for (j=0;j<=mm-1;j++)
        {
            xiv[j+1]=(double)(j-mm/2)*nris;
            ivet[j+1]=(double)(j-mm/2)*pi2/((double)mm*nris);
        }

        for (j=0;j<=mm-1;j++)
        {
            xi=xiv[j+1];

            caux=chi*chi;
            caux1=2.0*eps*eps;
            caux1=caux1*xi;
            caux1=caux1*ui;
            caux2=caux1+caux;

            zita=0.5*std::sqrt(caux2);
            caux1=std::exp(-2.0*tau*zita);

            beta=0.5*chi+zita;
            beta=beta+caux1*(zita-0.5*chi);

            gamma=1.0-caux1;

            caux=-ss*tau;
            caux2=caux*(zita-0.5*chi);
            caux=ss*std::log(2.0*(zita/beta));
            caux3=-v0*ui*xi*(gamma/beta);
            caux=caux+caux3;
            caux=caux+caux2;
            ff[j+1]=std::exp(caux);
        }

        sumr=0.0;
        //sumi=0.0;
        for (k=0;k<=mm-1;k++)
        {
            ip=i0-ivet[k+1];
            payoffval=payoff(ip);

            dxi=2.0*pi*(double)k/(double)mm*ui;
            csum=0.0;
            for (j=0;j<=mm-1;j++)
            {
                z=-(double)j*dxi;
                caux=std::pow(-1.0,j);
                csum=csum+ff[j+1]*caux*std::exp(z);
            }
            csum=csum*std::pow(-1.0,k)*nris/pi2;

            sumr=sumr+payoffval*std::real(csum);
            //sumi=sumi+payoffval*std::imag(csum);
        }
        sumr=sumr*nris;
        //sumi=sumi*nris;

        option=std::exp(-rtax*tau)*sumr;
        //impart=sumi;
        //QL_ENSURE(impart <= 1e-3,
        //          "imaginary part option (must be close to zero) = " << impart);
        return option;
    }

    struct payoff_adapter {
        ext::shared_ptr<QuantLib::Payoff> payoff;
        explicit payoff_adapter(ext::shared_ptr<QuantLib::Payoff> payoff)
        : payoff(payoff) {}
        Real operator()(Real S) const {
            return (*payoff)(S);
        }
    };

    }

    IntegralHestonVarianceOptionEngine::IntegralHestonVarianceOptionEngine(
                              const ext::shared_ptr<HestonProcess>& process)
    : process_(process) {
        registerWith(process_);
    }

    void IntegralHestonVarianceOptionEngine::calculate() const {

        QL_REQUIRE(process_->dividendYield().empty(),
                   "this engine does not manage dividend yields");

        Handle<YieldTermStructure> riskFreeRate = process_->riskFreeRate();

        Real epsilon = process_->sigma();
        Real chi = process_->kappa();
        Real theta = process_->theta();
        Real rho = process_->rho();
        Real v0 = process_->v0();

        Time tau = riskFreeRate->dayCounter().yearFraction(
                                        Settings::instance().evaluationDate(),
                                        arguments_.maturityDate);
        Rate r = riskFreeRate->zeroRate(arguments_.maturityDate,
                                        riskFreeRate->dayCounter(),
                                        Continuous);

        ext::shared_ptr<PlainVanillaPayoff> plainPayoff =
            ext::dynamic_pointer_cast<PlainVanillaPayoff>(arguments_.payoff);
        if (plainPayoff && plainPayoff->optionType() == Option::Call) {
            // a specialization for Call options is available
            Real strike = plainPayoff->strike();
            results_.value = IvopOneDim(epsilon, chi, theta, rho,
                                        v0, strike, tau, r)
                * arguments_.notional;
        } else {
            results_.value = IvopTwoDim(epsilon, chi, theta, rho, v0, tau, r,
                                        payoff_adapter(arguments_.payoff))
                * arguments_.notional;
        }
    }

}