/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Row;
using arma::span;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenRowVecElemIndRangeGenRowVec: public Expected {
    public:
      ExpectedInPlaceGenRowVecElemIndRangeGenRowVec() {
        cout << "Compute ExpectedInPlaceGenRowVecElemIndRangeGenRowVec(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters( {
            InputClass::GenRowVec,
            InputClass::ElemIndRange,
            InputClass::GenRowVec
          });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genRowVecA = *static_cast<Row<double>*>(value.second);
                break;
              case 1:
                _fileSuffix += "," + value.first;
                _elemIndRange = *static_cast<span*>(value.second);
                break;
              case 2:
                _fileSuffix += "," + value.first;
                _genRowVecB = *static_cast<Row<double>*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          _copyOfGenRowVecA = _genRowVecA;
          _copyOfElemIndRange = _elemIndRange;
          _copyOfGenRowVecB = _genRowVecB;

          expectedRowVecColsEqual();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecColsPlus();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecColsMinus();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecColsElemTimes();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecColsElemDivide();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecSubvecEqual();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecSubvecPlus();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecSubvecMinus();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecSubvecElemTimes();

          _genRowVecA = _copyOfGenRowVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genRowVecB = _copyOfGenRowVecB;
          expectedRowVecSubvecElemDivide();
        }

        cout << "done." << endl;
      }

    protected:
      Row<double> _genRowVecA;
      Row<double> _copyOfGenRowVecA;

      span _elemIndRange;
      span _copyOfElemIndRange;

      Row<double> _genRowVecB;
      Row<double> _copyOfGenRowVecB;

      void expectedRowVecColsEqual() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecColsEqual() ... ";

        _genRowVecA.cols(_elemIndRange.a, _elemIndRange.b) = _genRowVecB;

        save<double>("Row.colsEqual", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecColsPlus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecColsPlus() ... ";

        _genRowVecA.cols(_elemIndRange.a, _elemIndRange.b) += _genRowVecB;

        save<double>("Row.colsPlus", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecColsMinus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecColsMinus() ... ";

        _genRowVecA.cols(_elemIndRange.a, _elemIndRange.b) -= _genRowVecB;

        save<double>("Row.colsMinus", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecColsElemTimes() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecColsElemTimes() ... ";

        _genRowVecA.cols(_elemIndRange.a, _elemIndRange.b) %= _genRowVecB;

        save<double>("Row.colsElemTimes", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecColsElemDivide() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecColsElemDivide() ... ";

        _genRowVecA.cols(_elemIndRange.a, _elemIndRange.b) /= _genRowVecB;

        save<double>("Row.colsElemDivide", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecSubvecEqual() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecSubvecEqual() ... ";

        _genRowVecA.subvec(_elemIndRange.a, _elemIndRange.b) = _genRowVecB;

        save<double>("Row.subvecEqual", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecSubvecPlus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecSubvecPlus() ... ";

        _genRowVecA.subvec(_elemIndRange.a, _elemIndRange.b) += _genRowVecB;

        save<double>("Row.subvecPlus", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecSubvecMinus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecSubvecMinus() ... ";

        _genRowVecA.subvec(_elemIndRange.a, _elemIndRange.b) -= _genRowVecB;

        save<double>("Row.subvecMinus", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecSubvecElemTimes() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genRowVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedRowVecSubvecElemTimes() ... ";

        _genRowVecA.subvec(_elemIndRange.a, _elemIndRange.b) %= _genRowVecB;

        save<double>("Row.subvecElemTimes", _genRowVecA);
        cout << "done." << endl;
      }

      void expectedRowVecSubvecElemDivide() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genRowVecA.in_range(_elemIndRange) || _genRowVecB.n_elem != (_elemIndRange.b - _elemIndRange.a) + 1) {
          return;
        }

        cout << "- Compute expectedRowVecSubvecElemDivide() ... ";

        _genRowVecA.subvec(_elemIndRange.a, _elemIndRange.b) /= _genRowVecB;

        save<double>("Row.subvecElemDivide", _genRowVecA);
        cout << "done." << endl;
      }

  };
}


/*!
  @file utils.hpp
  @author Klaus K. Holst
  @copyright 2020, Klaus Kähler Holst

  @brief Various utility functions and constants

*/
#pragma once

#ifndef ARMA_R
#include <armadillo>
#endif
#if defined(ARMA_R)
#include <RcppArmadillo.h>
#endif
#include <cmath>
#include <complex>
#include <cfloat>      // precision of double (DBL_MIN)
#include <functional>  // std::bind for using non-static member function as argument to free function

namespace target {
  using cx_dbl  = std::complex<double>;
  using cx_func = std::function<arma::cx_mat(arma::cx_vec theta)>;

  arma::mat deriv(cx_func f,
                  arma::vec theta);

  arma::umat clusterid(const arma::uvec &id);
  arma::mat groupsum(const arma::mat &x,
                     const arma::uvec &cluster,
                     bool reduce = true);
  void fastpattern(const arma::umat &y,
                   arma::umat &pattern,
		   arma::uvec &group,
		   unsigned categories = 2);
  arma::umat fastapprox(arma::vec &time,  // sorted times
                        const arma::vec &newtime,
                        bool equal = false,
			// type: (0: nearedst, 1: right, 2: left)
                        unsigned type = 0);

  double SupTest(const arma::vec &D);
  double L2Test(const arma::vec &D,
		const arma::vec &t);
  double CramerVonMises(const arma::vec &x,
			const arma::vec &G);

  extern arma::mat const EmptyMat;
  extern arma::vec const EmptyVec;

  extern const char* COL_RESET;
  extern const char* COL_DEF;
  extern const char* BLACK;
  extern const char* RED;
  extern const char* MAGENTA;
  extern const char* YELLOW;
  extern const char* GREEN;
  extern const char* BLUE;
  extern const char* CYAN;
  extern const char* WHITE;
  extern const char* GRAY;
  extern const char* LRED;
  extern const char* LGREEN;
  extern const char* LYELLOW;
  extern const char* LBLUE;
  extern const char* LMAGENTA;
  extern const char* LCYAN;
  extern const char* LWHITE;

}  // namespace target


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef PSHAPEFUNCTIONS_HH
#define PSHAPEFUNCTIONS_HH

#include <tuple>
#include <vector>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <memory>

#include <boost/multi_array.hpp>

#include "fem/barycentric.hh"
#include "fem/fixdune.hh"
#include "fem/mllgeometry.hh"
#include "linalg/dynamicMatrix.hh"

namespace Kaskade
{
  /** \ingroup fem
   * \brief Shape functions
   *
   * This class provides a common interface to the access of shape functions
   *
   * \tparam ctype the scalar type used for coordinates (usually double)
   * \tparam dim the spatial dimension
   * \tparam T  Scalar type, the value field type of the shape functions
   * \tparam comp number of components
   *
   * A shape function lives on the reference element and may be scalar valued or vector valued:
   * \f[ \phi: R^d \to T^m \f]
   * with the spatial (grid) dimension \f$ d \f$, the value field type \f$ T \f$ (either a complex
   * or a real type), and the vectorial dimension \f$ m \f$ (comp) of the image space. Shape function
   * values are indeed defined not only on the reference element but on whole \f$ R^d \f$, even though
   * the values outside the reference elements may be of little interest.
   * It is possible to evaluate the shape function and its derivative.
   *
   */
  template <class ctype, int dim, class T=double, int comp=1>
  class ShapeFunction
  {
  public:
    virtual ~ShapeFunction() {}

    /**
     * \brief Creates a copy of the actual derived shape function.
     */
    virtual std::unique_ptr<ShapeFunction> clone() const = 0;

    /**
     * \brief Evaluates the shape function (all components at once).
     */
    virtual Dune::FieldVector<T,comp> evaluateFunction(Dune::FieldVector<ctype,dim> const& xi) const = 0;

    /**
     * \brief Evaluates the derivative of the shape function (all components and all directions at once).
     */
    virtual Dune::FieldMatrix<T,comp,dim> evaluateDerivative(Dune::FieldVector<ctype,dim> const& xi) const = 0;

    /**
     * \brief Evaluates the second derivative of the shape function (all components and all directions at once).
     */
    virtual Tensor3<T,comp,dim,dim> evaluate2ndDerivative(Dune::FieldVector<ctype,dim> const& xi) const {
      std::cerr << "NOT IMPLEMENTED: " << __FILE__ << ":" << __LINE__ << "\n";
      abort();
    }

    /**
     * \brief Returns a tuple (nominalOrder,codim,entity,index) giving detailed information about the location of the shape function.
     *
     * Each shape function is associated to a certain subentity of the element.
     *
     * nominalOrder is a nonnegative ordering parameter that is usually
     * the polynomial order of the shape function, but need not coincide.
     *
     * codim is the codimension of the subentity to which the shape
     * function is associated, entity is the number of the subentity,
     * and index is the number of the shape function among those that
     * are associated to the same subentity.
     */
    virtual std::tuple<int,int,int,int> location() const = 0;
  };

  //---------------------------------------------------------------------
  /** \ingroup fem
   *  \brief A set of shape functions
   *
   *
   * \tparam ctype scalar type for coordinates
   * \tparam dimension spatial dimension
   * \tparam T  scalar type
   * \tparam comp number of components
   */
  template <class ctype, int dimension, class T, int comp=1>
  class ShapeFunctionSet
  {
  public:
    /// Scalar field type
    typedef T Scalar;
    /// type of one shape function
    typedef ShapeFunction<ctype,dimension,T,comp> value_type;

    /**
     * \brief A matrix type mapping one-component coefficient vectors.
     */
    typedef DynamicMatrix<Dune::FieldMatrix<T,1,1> > Matrix;

    /// Constructor
    ShapeFunctionSet(Dune::GeometryType gt):
      order_(-1), size_(-1),
      gt_(gt),
      refElem(&Dune::ReferenceElements<ctype,dimension>::general(gt))
    {}

    ShapeFunctionSet(ShapeFunctionSet const& other)
    : iNodes(other.iNodes), projection(other.projection), order_(other.order_), size_(other.size_),
      gt_(other.gt_), refElem(&Dune::ReferenceElements<ctype,dimension>::general(gt_))
    {}

    /**
     * \brief Number of components of the shape function values.
     */
    static int const comps = comp;

    virtual ~ShapeFunctionSet() {}

    /// Random access to a shape function
    virtual value_type const& operator[](int i) const = 0;

    /**
     * \brief Number of shape functions in the set.
     */
    virtual int size() const { return size_; }

    /**
     * \brief Maximal polynomial order of shape functions.
     */
    int order() const { return order_; }

    /// Type of geometry on which the shape functions are defined.
    Dune::GeometryType type() const { return gt_; }

    /**
     * Returns a reference to the reference element on which this shape
     * function set is defined.
     */
    Dune::ReferenceElement<ctype,dimension> const& referenceElement() const { return *refElem; }

    /// A container type for holding interpolation points in the reference elements.
    typedef std::vector<Dune::FieldVector<ctype,dimension> > InterpolationNodes;

    /// A twodimensional array type for holding shape function values
    /// evaluated at a set of nodes.
    typedef DynamicMatrix<Dune::FieldMatrix<T,comp,1> > SfValueArray;

    /**
     * \brief Initialize the hierarchical projection matrix based on the given
     * lower order shape function set.
     */
    void initHierarchicalProjection(ShapeFunctionSet<ctype,dimension,T,comp> const* sfl)
    {
      // If no lower order shape function set is given, we assume an
      // empty set, i.e. the projection is just zero.
      if (!sfl || size()==0) {
        projection.setSize(size(),size());
//       projection = 0 // causes compilation error with dune-2.4.0 and clang++ on OS X (Darwin)
        projection.fill(0);
        return;
      }

      // Compute the hierarchic projection P = I_h E_l(x_h) I_l E_h(x_l),
      // where E_h(x_l) the evaluation of high order shape functions at
      // low order nodes, I_l the low order interpolation, E_l(x_h) the
      // evaluation of low order shape functions at high order nodes,
      // and I_h the high order shape function evaluation.

      // Compute the local restriction matrix A
      SfValueArray Ehxl;
      evaluate(sfl->interpolationNodes(),Ehxl);  // E_h(x_l)
      Matrix A;
      sfl->interpolate(Ehxl,A);                  // I_l  E_h(x_l)

      // Compute the local prolongation matrix B
      SfValueArray Elxh;
      sfl->evaluate(interpolationNodes(),Elxh);  // E_l(x_h)
      Matrix B;
      interpolate(Elxh,B);                       // I_h E_l(x_h)

      // Compute projection P=B*A
      MatMult(projection,B,A);
    }

    /**
     * @brief Interpolation points.
     *
     * Provides interpolation points such
     * that the shape function coefficients can be computed from
     * function values at interpolation nodes by multiplication by this
     * matrix.  The interpolation points are guaranteed to be inside the
     * reference element associated to this shape function set.
     */
    InterpolationNodes const& interpolationNodes() const { return iNodes; }

    /**
     * @brief Left-multiplies the provided matrix with the interpolation
     * matrix of the shape function set.
     *
     * Each column of A is interpreted as values of some function
     * evaluated at this shape function set's interpolation points (see
     * below). The columns of the output array IA then contain the shape
     * function coefficients such that the corresponding
     * linearcombination of shape functions "interpolates" that function
     * in the interpolation points. What "interpolation" means is up to
     * the actual implementation.
     *
     * IA is automatically resized if needed.
     *
     * Storage order: A[i][j] contains the value of function j at
     * interpolation node i.
     */
    virtual void interpolate(SfValueArray const& A, Matrix& IA) const = 0;

    /**
     * @brief Evaluate shape function set at a set of points. In
     * notation of the LocalToGlobalMapperConcept, this gives the matrix
     * \f$ \Phi \f$: the entry Phi[i][j] is the value of shape function
     * j evaluated at iNodes[i].
     *
     * @param iNodes the points at which the shape functions are to be evaluated.
     * \param phi    the array that is filled with shape function values. The array
     *               will be resized if needed.
     */
    void evaluate(InterpolationNodes const& iNodes, SfValueArray& phi) const
    {
      int s = size();
      phi.setSize(iNodes.size(),s);

      for (int j=0; j<s; ++j) {
        value_type const& sf = (*this)[j];
        for (int i=0; i<iNodes.size(); ++i) {
          Dune::FieldVector<T,comp> v = sf.evaluateFunction(iNodes[i]);
          for (int k=0; k<comp; ++k)
            phi[i][j][k] = v[k];  // todo: improve efficiency! cache directly!
        }
      }
    }

    /**
     * \brief Returns a square matrix that projects shape function
     * coefficients to a subspace spanned by shape functions of lower
     * order.  This is intended to be used to implement embedded error
     * estimators. The actual definition of this subspace depends on the
     * shape function set specified in the call to
     * initHierarchicalProjection().
     */
    Matrix const& hierarchicProjection() const
    {
      // check that the projection has been properly initialized.
      assert(projection.N()==size() && projection.M()==size());

      return projection;
    }

    virtual void removeShapeFunction(size_t index) {}

  protected:
    InterpolationNodes iNodes;
    Matrix             projection;
    int                order_;
    int                size_;

  private:
    Dune::GeometryType gt_;
    Dune::ReferenceElement<ctype,dimension> const* refElem;
   };

  //---------------------------------------------------------------------
  /**
   * \brief Restricted shape function set.
   * Introduces a new local ordering for the shape functions. To retrieve the original shape function id use getId(int newLocalId)
   * \todo docme: what is is good for? Which ordering? Why "restricted"?
   */
  template <class ShapeFunctionSet_>
  class RestrictedShapeFunctionSet: public ShapeFunctionSet_
  {
  public:
    /// Grid type
    //typedef typename ShapeFunctionSet_::Grid Grid; causes compiler error
    /// Scalar field type
    typedef typename ShapeFunctionSet_::Scalar Scalar;
    /// type of one shape function
    typedef typename ShapeFunctionSet_::value_type value_type;

    explicit RestrictedShapeFunctionSet(int order) : ShapeFunctionSet_(order) {}

    RestrictedShapeFunctionSet(ShapeFunctionSet_ const& other) : ShapeFunctionSet_(other) {}
    RestrictedShapeFunctionSet(RestrictedShapeFunctionSet const& other) : ShapeFunctionSet_(other) {}
    /// Copy constructor. Resets restriction ids
    RestrictedShapeFunctionSet(RestrictedShapeFunctionSet const& other, std::vector<int>* ids_) : ShapeFunctionSet_(other) {}

    virtual ~RestrictedShapeFunctionSet(){}

    /**
     * \param ids_ vector of used ids of the shape function set
     */
    void setRestriction(std::vector<int> const& ids_)
    {
      if(ids_.size()==0)
      {
        this->sf.clear();
        this->iNodes.clear();
        this->size_ = 0;
        return;
      }

      for(int i=this->sf.size()-1; i>-1; --i)
        if(std::find(ids_.begin(),ids_.end(),i)==ids_.end()) this->removeShapeFunction(i);

      this->size_ = ids_.size();
    }
  };

  //---------------------------------------------------------------------

  /**
   * \brief Base class for sets of shape function containers.
   *
   * \tparam ctype scalar type for coordinates
   * \tparam dimension spatial dimension
   * \tparam T the scalar shape function return value
   * \tparam comp the number of components of the shape functions' values
   */
  template <class ctype, int dimension, class T, int comp=1>
  class ShapeFunctionSetContainer
  {
  public:
    typedef ShapeFunctionSet<ctype,dimension,T,comp> value_type;

    virtual ~ShapeFunctionSetContainer() {}

    /// access a shape function via type and order
    virtual value_type const& operator() (Dune::GeometryType type, int order) const = 0;
  };

  //---------------------------------------------------------------------
  //---------------------------------------------------------------------

  /**
   * For a given shape function set \arg sfs which is defined on a
   * simplex and permits a simple coupling, and a given permutation \arg
   * vPermutation of the vertices of the simplex defining an affine
   * spatial transformation \f$ f \f$, this function returns a
   * permutation \arg sfPermutation of the shape functions and a sign
   * vector \arg sign, such that
   *
   * \f[ \mathrm{sign[k]}\phi_{\mathrm{sfPermutation[k]}}(f(x)) =
   * \phi_k(x). \f]
   */
  template <class ShapeFunctionSet>
  void computeSimplexSfPermutation(ShapeFunctionSet const& sfs,
      int const* vPermutation,
      int*       sfPermutation,
      int*       sign)
  {
    assert(vPermutation);
    assert(sfPermutation);
    assert(sign);

    // Ok, we rely on the evaluation & interpolation of the shape
    // function set. First we evaluate the shape functions on the
    // interpolation nodes transformed by f, then we interpolate these
    // values. This gives us a matrix which, if the shape function set
    // indeed allows simple coupling, is a signed permutation
    // matrix. Finally we extract the signs and the permutation from
    // this matrix.

    // Get the interpolation nodes. Make a copy because we need to
    // modify them.
    typedef typename ShapeFunctionSet::InterpolationNodes InterpolationNodes;
    InterpolationNodes in = sfs.interpolationNodes();

    // Transform the nodes. This is done by permuting their barycentric
    // coordinates. Remember that the interpolation nodes are given in
    // Cartesian, not in barycentric coordinates. This determines the
    // loop termination for j.
    for (int i=0; i<in.size(); ++i) {
      Dune::FieldVector<typename ShapeFunctionSet::Grid::ctype,ShapeFunctionSet::Grid::dimension+1> b = barycentric(in[i]);
      for (int j=0; j<ShapeFunctionSet::Grid::dimension; ++j)
        in[i][j] = b[vPermutation[j]];
    }

    // Evaluate the shape functions at the transformed nodes.
    typename ShapeFunctionSet::SfValueArray phi(in.size(),sfs.size());
    sfs.evaluate(in,phi);

    // Interpolate the shape function values.
    typename ShapeFunctionSet::Matrix P(sfs.size(),sfs.size());
    sfs.interpolate(phi,P);

#ifndef NDEBUG
    // Check that the permutation matrix is indeed a signed permutation
    bool ok = true;
    for (int i=0; i<sfs.size(); ++i) {
      double rowSum = 0;
      double colSum = 0;
      for (int j=0; j<sfs.size(); ++j) {
        if (std::abs(P[i][j])>1e-8 && std::abs(1-std::abs(P[i][j]))>1e-8) ok = false;
        rowSum += std::abs(P[i][j]);
        colSum += std::abs(P[j][i]);
      }
      if (std::abs(rowSum-1)>1e-8 || std::abs(colSum-1)>1e-8) ok = false;
    }
    if (!ok) {
      std::cout << "\nPermutation matrix:\n";
      for (int i=0; i<P.N(); ++i) {
        for (int j=0; j<P.M(); ++j)
          std::cout << std::setw(2) << P[i][j] << " ";
        std::cout << "\n";
      }
    }
#endif

    // Extract the permutation and the sign.
    for (int i=0; i<sfs.size(); ++i)
      for (int j=0; j<sfs.size(); ++j)
        if (std::abs(P[j][i])>0.5) {
          sfPermutation[i] = j; // XXX or the other way round?
          sign[i] = P[j][i]>0? 1: -1;
          break;
        }

  }
} // end of namespace Kaskade

#endif


#ifndef _BOOSTSPATIALINDEX_SPATIALINDEX
#define _BOOSTSPATIALINDEX_SPATIALINDEX

#include <v8.h>
#include <node.h>
#include <nan.h>
#include <unordered_map>
#include <vector>
#include <string>
#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "shape.hpp"

typedef std::pair<box, std::shared_ptr<Shape>> treeValue;

const std::string toString(v8::Isolate *isolate, v8::Local<v8::Value> value) {
#if NODE_MODULE_VERSION < NODE_12_0_MODULE_VERSION
    v8::String::Utf8Value utf8(value);
#else
    v8::String::Utf8Value utf8(isolate, value);
#endif

    return *utf8;
}

class SpatialIndex : public Nan::ObjectWrap {
  public:
    static NAN_MODULE_INIT(init);

  private:
    explicit SpatialIndex();
    ~SpatialIndex();

    // Node bridge handling
    static Nan::Persistent<v8::Function> constructor;
    static NAN_METHOD(New);

    // spatial index related methods
    static NAN_METHOD(addBBox);
    static NAN_METHOD(addCircle);
    static NAN_METHOD(addAnnulus);
    static NAN_METHOD(addPolygon);
    static NAN_METHOD(queryPoint);
    static NAN_METHOD(queryIntersect);
    static NAN_METHOD(remove);

    // The spatial index containing MBRs (minimum bounding rectangles)
    boost::geometry::index::rtree< treeValue, boost::geometry::index::rstar<16> > rtree;

    // Map geometry objects with their corresponding string id
    std::unordered_map<std::string, std::shared_ptr<Shape> > repository;
};

#endif


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example eigen-with-viennacl.cpp
*
*   This tutorial shows how data can be directly transferred from the Eigen Library to ViennaCL objects using the built-in convenience wrappers.
*
*   The first step is to include the necessary headers and activate the Eigen convenience functions in ViennaCL:
**/

// System headers
#include <iostream>

// Eigen headers
#include <Eigen/Core>
#include <Eigen/Sparse>

// IMPORTANT: Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Eigen objects
#define VIENNACL_WITH_EIGEN 1

// ViennaCL includes
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"

// Helper functions for this tutorial:
#include "vector-io.hpp"

/**
*   The following is a set of auxiliary dispatchers for obtaining the right Eigen types for a given floating point type.
*   This is merely an implementation detail, so feel free to skip over it.
**/

//dense matrix:
template<typename T>
struct Eigen_dense_matrix
{
  typedef typename T::ERROR_NO_EIGEN_TYPE_AVAILABLE   error_type;
};

template<>
struct Eigen_dense_matrix<float>
{
  typedef Eigen::MatrixXf  type;
};

template<>
struct Eigen_dense_matrix<double>
{
  typedef Eigen::MatrixXd  type;
};

//sparse matrix
template<typename T>
struct Eigen_vector
{
  typedef typename T::ERROR_NO_EIGEN_TYPE_AVAILABLE   error_type;
};

template<>
struct Eigen_vector<float>
{
  typedef Eigen::VectorXf  type;
};

template<>
struct Eigen_vector<double>
{
  typedef Eigen::VectorXd  type;
};

/**
*    The following function contains the main code for this tutorial.
*    It consists of the following steps:
*      - Creates Eigen matrices and vectors
*      - Initializes them with data
*      - Create ViennaCL objects
*      - Copy them over to the respective ViennaCL objects
*      - Compute matrix-vector products in both Eigen and ViennaCL and compare results.
*
**/
template<typename ScalarType>
void run_tutorial()
{
  /**
  * Get Eigen matrix and vector types for the provided ScalarType.
  * Involves a little bit of template-metaprogramming.
  **/
  typedef typename Eigen_dense_matrix<ScalarType>::type  EigenMatrix;
  typedef typename Eigen_vector<ScalarType>::type        EigenVector;

  /**
  * Create and fill dense matrices from the Eigen library:
  **/
  EigenMatrix eigen_densemat(6, 5);
  EigenMatrix eigen_densemat2(6, 5);
  eigen_densemat(0,0) = 2.0;   eigen_densemat(0,1) = -1.0;
  eigen_densemat(1,0) = -1.0;  eigen_densemat(1,1) =  2.0;  eigen_densemat(1,2) = -1.0;
  eigen_densemat(2,1) = -1.0;  eigen_densemat(2,2) = -1.0;  eigen_densemat(2,3) = -1.0;
  eigen_densemat(3,2) = -1.0;  eigen_densemat(3,3) =  2.0;  eigen_densemat(3,4) = -1.0;
                               eigen_densemat(5,4) = -1.0;  eigen_densemat(4,4) = -1.0;
  Eigen::Map<EigenMatrix> eigen_densemat_map(eigen_densemat.data(), 6, 5); // same as eigen_densemat, but emulating user-provided buffer

  /**
  * Create and fill sparse matrices from the Eigen library:
  **/
  Eigen::SparseMatrix<ScalarType, Eigen::RowMajor> eigen_sparsemat(6, 5);
  Eigen::SparseMatrix<ScalarType, Eigen::RowMajor> eigen_sparsemat2(6, 5);
  eigen_sparsemat.reserve(5*2);
  eigen_sparsemat.insert(0,0) = 2.0;   eigen_sparsemat.insert(0,1) = -1.0;
  eigen_sparsemat.insert(1,1) = 2.0;   eigen_sparsemat.insert(1,2) = -1.0;
  eigen_sparsemat.insert(2,2) = -1.0;  eigen_sparsemat.insert(2,3) = -1.0;
  eigen_sparsemat.insert(3,3) = 2.0;   eigen_sparsemat.insert(3,4) = -1.0;
  eigen_sparsemat.insert(5,4) = -1.0;
  //eigen_sparsemat.endFill();

  /**
  * Create and fill a few vectors from the Eigen library:
  **/
  EigenVector eigen_rhs(5);
  Eigen::Map<EigenVector> eigen_rhs_map(eigen_rhs.data(), 5);
  EigenVector eigen_result(6);
  EigenVector eigen_temp(6);

  eigen_rhs(0) = 10.0;
  eigen_rhs(1) = 11.0;
  eigen_rhs(2) = 12.0;
  eigen_rhs(3) = 13.0;
  eigen_rhs(4) = 14.0;

  /**
  * Create the corresponding ViennaCL objects:
  **/
  viennacl::vector<ScalarType> vcl_rhs(5);
  viennacl::vector<ScalarType> vcl_result(6);
  viennacl::matrix<ScalarType> vcl_densemat(6, 5);
  viennacl::compressed_matrix<ScalarType> vcl_sparsemat(6, 5);

  /**
  * Directly copy the Eigen objects to ViennaCL objects
  **/
  viennacl::copy(&(eigen_rhs[0]), &(eigen_rhs[0]) + 5, vcl_rhs.begin());  // Method 1: via iterator interface (cf. std::copy())
  viennacl::copy(eigen_rhs, vcl_rhs);                                     // Method 2: via built-in wrappers (convenience layer)
  viennacl::copy(eigen_rhs_map, vcl_rhs);                                 // Same as method 2, but for a mapped vector

  viennacl::copy(eigen_densemat, vcl_densemat);
  viennacl::copy(eigen_densemat_map, vcl_densemat); //same as above, using mapped matrix
  viennacl::copy(eigen_sparsemat, vcl_sparsemat);
  std::cout << "VCL sparsematrix dimensions: " << vcl_sparsemat.size1() << ", " << vcl_sparsemat.size2() << std::endl;

  // For completeness: Copy matrices from ViennaCL back to Eigen:
  viennacl::copy(vcl_densemat, eigen_densemat2);
  viennacl::copy(vcl_sparsemat, eigen_sparsemat2);

  /**
  * Run dense matrix-vector products and compare results:
  **/
  eigen_result = eigen_densemat * eigen_rhs;
  vcl_result = viennacl::linalg::prod(vcl_densemat, vcl_rhs);
  viennacl::copy(vcl_result, eigen_temp);
  std::cout << "Difference for dense matrix-vector product: " << (eigen_result - eigen_temp).norm() << std::endl;
  std::cout << "Difference for dense matrix-vector product (Eigen->ViennaCL->Eigen): "
            << (eigen_densemat2 * eigen_rhs - eigen_temp).norm() << std::endl;

  /**
  * Run sparse matrix-vector products and compare results:
  **/
  eigen_result = eigen_sparsemat * eigen_rhs;
  vcl_result = viennacl::linalg::prod(vcl_sparsemat, vcl_rhs);
  viennacl::copy(vcl_result, eigen_temp);
  std::cout << "Difference for sparse matrix-vector product: " << (eigen_result - eigen_temp).norm() << std::endl;
  std::cout << "Difference for sparse matrix-vector product (Eigen->ViennaCL->Eigen): "
            << (eigen_sparsemat2 * eigen_rhs - eigen_temp).norm() << std::endl;
}

/**
*   In the main() routine we only call the worker function defined above with both single and double precision arithmetic.
**/
int main(int, char *[])
{
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Single precision" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  run_tutorial<float>();

#ifdef VIENNACL_HAVE_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Double precision" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    run_tutorial<double>();
  }

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;

}


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HISTOGRAM_ACCUMULATORS_MEAN_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_MEAN_HPP

#include <boost/core/nvp.hpp>
#include <boost/histogram/fwd.hpp> // for mean<>
#include <boost/throw_exception.hpp>
#include <cassert>
#include <stdexcept>
#include <type_traits>

namespace boost {
namespace histogram {
namespace accumulators {

/** Calculates mean and variance of sample.

  Uses Welfords's incremental algorithm to improve the numerical
  stability of mean and variance computation.
*/
template <class ValueType>
class mean {
public:
  using value_type = ValueType;
  using const_reference = const value_type&;

  mean() = default;

  /// Allow implicit conversion from mean<T>
  template <class T>
  mean(const mean<T>& o) noexcept
      : sum_{o.sum_}, mean_{o.mean_}, sum_of_deltas_squared_{o.sum_of_deltas_squared_} {}

  /// Initialize to external count, mean, and variance
  mean(const_reference n, const_reference mean, const_reference variance) noexcept
      : sum_(n), mean_(mean), sum_of_deltas_squared_(variance * (n - 1)) {}

  /// Insert sample x
  void operator()(const_reference x) noexcept {
    sum_ += static_cast<value_type>(1);
    const auto delta = x - mean_;
    mean_ += delta / sum_;
    sum_of_deltas_squared_ += delta * (x - mean_);
  }

  /// Insert sample x with weight w
  void operator()(const weight_type<value_type>& w, const_reference x) noexcept {
    sum_ += w.value;
    const auto delta = x - mean_;
    mean_ += w.value * delta / sum_;
    sum_of_deltas_squared_ += w.value * delta * (x - mean_);
  }

  /// Add another mean accumulator
  mean& operator+=(const mean& rhs) noexcept {
    if (sum_ != 0 || rhs.sum_ != 0) {
      const auto tmp = mean_ * sum_ + rhs.mean_ * rhs.sum_;
      sum_ += rhs.sum_;
      mean_ = tmp / sum_;
    }
    sum_of_deltas_squared_ += rhs.sum_of_deltas_squared_;
    return *this;
  }

  /** Scale by value

   This acts as if all samples were scaled by the value.
  */
  mean& operator*=(const_reference s) noexcept {
    mean_ *= s;
    sum_of_deltas_squared_ *= s * s;
    return *this;
  }

  bool operator==(const mean& rhs) const noexcept {
    return sum_ == rhs.sum_ && mean_ == rhs.mean_ &&
           sum_of_deltas_squared_ == rhs.sum_of_deltas_squared_;
  }

  bool operator!=(const mean& rhs) const noexcept { return !operator==(rhs); }

  /// Return how many samples were accumulated
  const_reference count() const noexcept { return sum_; }

  /// Return mean value of accumulated samples
  const_reference value() const noexcept { return mean_; }

  /// Return variance of accumulated samples
  value_type variance() const noexcept { return sum_of_deltas_squared_ / (sum_ - 1); }

  template <class Archive>
  void serialize(Archive& ar, unsigned version) {
    if (version == 0) {
      // read only
      std::size_t sum;
      ar& make_nvp("sum", sum);
      sum_ = static_cast<value_type>(sum);
    } else {
      ar& make_nvp("sum", sum_);
    }
    ar& make_nvp("mean", mean_);
    ar& make_nvp("sum_of_deltas_squared", sum_of_deltas_squared_);
  }

private:
  value_type sum_{};
  value_type mean_{};
  value_type sum_of_deltas_squared_{};
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED

namespace boost {
namespace serialization {

template <class T>
struct version;

// version 1 for boost::histogram::accumulators::mean<T>
template <class T>
struct version<boost::histogram::accumulators::mean<T>> : std::integral_constant<int, 1> {
};

} // namespace serialization
} // namespace boost

namespace std {
template <class T, class U>
/// Specialization for boost::histogram::accumulators::mean.
struct common_type<boost::histogram::accumulators::mean<T>,
                   boost::histogram::accumulators::mean<U>> {
  using type = boost::histogram::accumulators::mean<common_type_t<T, U>>;
};
} // namespace std

#endif

#endif


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

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

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

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include "toplevelfixture.hpp"
#include <boost/assign/list_of.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include <ql/math/interpolations/all.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>

#include <qle/termstructures/pricecurve.hpp>

using namespace std;

using namespace boost::unit_test_framework;
using namespace boost::assign;

using namespace QuantLib;
using namespace QuantExt;

namespace {

class CommonData {
public:
    // Variables
    vector<Date> testDates;
    vector<Period> curveTenors;
    vector<Real> prices;
    vector<Real> shiftedPrices;
    vector<boost::shared_ptr<SimpleQuote> > pQuotes;
    vector<Handle<Quote> > quotes;

    // Dates on which we will check interpolated values
    vector<Date> interpolationDates;
    vector<Time> interpolationTimes;
    // Expected (linear) interpolation results on first test date
    vector<Real> baseExpInterpResults;
    // Expected (linear) interpolation results for floating reference date
    // curve after moving to second test date and requesting price by date
    vector<Real> afterExpInterpResults;
    // Expected (linear) interpolation results on first test date after
    // shifting quotes
    vector<Real> baseNewInterpResults;
    // Expected (linear) interpolation results for floating reference date
    // curve after shifting quotes and moving to second test date and
    // requesting price by date
    vector<Real> afterNewInterpResults;

    // Expected loglinear interpolation results
    vector<Real> expLogInterpResults;

    DayCounter curveDayCounter;
    Real tolerance;
    bool extrapolate;

    // cleanup
    SavedSettings backup;

    // Default constructor
    CommonData()
        : testDates(2), curveTenors(6), prices(6), shiftedPrices(6), pQuotes(6), quotes(6), interpolationDates(3),
          interpolationTimes(3), baseExpInterpResults(3), afterExpInterpResults(3), baseNewInterpResults(3),
          afterNewInterpResults(3), expLogInterpResults(3), curveDayCounter(Actual365Fixed()), tolerance(1e-10),
          extrapolate(true) {

        // Evaluation dates on which tests will be performed
        testDates[0] = Date(15, Feb, 2018);
        testDates[1] = Date(15, Mar, 2018);

        // Curve tenors and prices
        curveTenors[0] = 0 * Days;
        prices[0] = 14.5;
        shiftedPrices[0] = 16.6;
        curveTenors[1] = 6 * Months;
        prices[1] = 16.7;
        shiftedPrices[1] = 19.9;
        curveTenors[2] = 1 * Years;
        prices[2] = 19.9;
        shiftedPrices[2] = 24.4;
        curveTenors[3] = 18 * Months;
        prices[3] = 24.5;
        shiftedPrices[3] = 31.3;
        curveTenors[4] = 2 * Years;
        prices[4] = 28.5;
        shiftedPrices[4] = 38.1;
        curveTenors[5] = 5 * Years;
        prices[5] = 38.8;
        shiftedPrices[5] = 54.5;

        // Create quotes
        for (Size i = 0; i < quotes.size(); i++) {
            pQuotes[i] = boost::make_shared<SimpleQuote>(prices[i]);
            quotes[i] = Handle<Quote>(pQuotes[i]);
        }

        // Interpolation variables
        interpolationDates[0] = Date(1, Jan, 2019);
        interpolationDates[1] = Date(1, Jun, 2021);
        interpolationDates[2] = Date(1, Aug, 2025);

        for (Size i = 0; i < interpolationDates.size(); i++) {
            interpolationTimes[i] = curveDayCounter.yearFraction(testDates[0], interpolationDates[i]);
        }

        baseExpInterpResults[0] = 19.1173913043478;
        baseExpInterpResults[1] = 32.9357664233577;
        baseExpInterpResults[2] = 47.2392335766423;

        afterExpInterpResults[0] = 18.6304347826087;
        afterExpInterpResults[1] = 32.6726277372263;
        afterExpInterpResults[2] = 46.9760948905109;

        baseNewInterpResults[0] = 23.2994565217391;
        baseNewInterpResults[1] = 45.1627737226277;
        baseNewInterpResults[2] = 67.9372262773723;

        afterNewInterpResults[0] = 22.6146739130435;
        afterNewInterpResults[1] = 44.7437956204380;
        afterNewInterpResults[2] = 67.5182481751825;

        expLogInterpResults[0] = 19.0648200765280;
        expLogInterpResults[1] = 32.5497181830507;
        expLogInterpResults[2] = 49.9589077461237;
    }

    // Give curve dates for one of the two test dates
    vector<Date> dates(Size testDatesIdx) const {
        vector<Date> result(curveTenors.size());
        for (Size i = 0; i < result.size(); i++) {
            result[i] = testDates[testDatesIdx] + curveTenors[i];
        }
        return result;
    }

    // Give curve times for one of the two test dates
    vector<Time> times(Size testDatesIdx) const {
        vector<Time> result(curveTenors.size());
        for (Size i = 0; i < result.size(); i++) {
            Date maturity = testDates[testDatesIdx] + curveTenors[i];
            result[i] = curveDayCounter.yearFraction(testDates[testDatesIdx], maturity);
        }
        return result;
    }

    // Update quotes with new shifted prices
    void updateQuotes() const {
        for (Size i = 0; i < quotes.size(); i++) {
            pQuotes[i]->setValue(shiftedPrices[i]);
        }
    }
};

// Perform some common curve checks on the first test date
template <class I> void commonChecks(CommonData& td, InterpolatedPriceCurve<I>& priceCurve, bool isLogLinear = false) {
    BOOST_TEST_MESSAGE("Performing common curve checks");

    // Check the prices at the pillar times
    for (Size i = 0; i < td.times(0).size(); i++) {
        BOOST_CHECK_CLOSE(td.prices[i], priceCurve.price(td.times(0)[i], td.extrapolate), td.tolerance);
    }

    // Check the prices at the pillar dates
    for (Size i = 0; i < td.dates(0).size(); i++) {
        BOOST_CHECK_CLOSE(td.prices[i], priceCurve.price(td.dates(0)[i], td.extrapolate), td.tolerance);
    }

    // Check some interpolated & extrapolated values
    vector<Real> expResults = isLogLinear ? td.expLogInterpResults : td.baseExpInterpResults;

    for (Size i = 0; i < td.interpolationDates.size(); i++) {
        BOOST_CHECK_CLOSE(expResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate), td.tolerance);
        BOOST_CHECK_CLOSE(expResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate), td.tolerance);
    }
}

} // namespace

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(PriceCurveTest)

BOOST_AUTO_TEST_CASE(testTimesAndPricesCurve) {

    BOOST_TEST_MESSAGE("Testing interpolated price curve built from times and prices");

    CommonData td;

    // Look at the first test date
    Settings::instance().evaluationDate() = td.testDates[0];

    // Create a linearly interpolated price curve
    vector<Time> times = td.times(0);
    InterpolatedPriceCurve<Linear> priceCurve(times, td.prices, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, priceCurve, false);

    // Create a loglinearly interpolated price curve
    InterpolatedPriceCurve<LogLinear> logPriceCurve(times, td.prices, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, logPriceCurve, true);

    // Check linearly interpolated price curve after moving reference date
    Settings::instance().evaluationDate() = td.testDates[1];

    // Check curve reference date is now second test date
    BOOST_CHECK_EQUAL(priceCurve.referenceDate(), td.testDates[1]);

    // Requesting price by time should give the same results as previously
    // Requesting by date should give new results (floating reference date curve)
    for (Size i = 0; i < td.interpolationTimes.size(); i++) {
        BOOST_CHECK_CLOSE(td.baseExpInterpResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate),
                          td.tolerance);
        BOOST_CHECK_CLOSE(td.afterExpInterpResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate),
                          td.tolerance);
    }
}

BOOST_AUTO_TEST_CASE(testTimesAndQuotesCurve) {

    BOOST_TEST_MESSAGE("Testing interpolated price curve built from times and quotes");

    CommonData td;

    // Look at the first test date
    Settings::instance().evaluationDate() = td.testDates[0];

    // Create a linearly interpolated price curve
    vector<Time> times = td.times(0);
    InterpolatedPriceCurve<Linear> priceCurve(times, td.quotes, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, priceCurve, false);

    // Create a loglinearly interpolated price curve
    InterpolatedPriceCurve<LogLinear> logPriceCurve(times, td.quotes, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, logPriceCurve, true);

    // Check linearly interpolated price curve after moving reference date
    Settings::instance().evaluationDate() = td.testDates[1];

    // Check curve reference date is now second test date
    BOOST_CHECK_EQUAL(priceCurve.referenceDate(), td.testDates[1]);

    // Requesting price by time should give the same results as previously
    // Requesting by date should give new results (floating reference date curve)
    for (Size i = 0; i < td.interpolationTimes.size(); i++) {
        BOOST_CHECK_CLOSE(td.baseExpInterpResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate),
                          td.tolerance);
        BOOST_CHECK_CLOSE(td.afterExpInterpResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate),
                          td.tolerance);
    }

    // Update quotes and check interpolations again (on this second test date)
    td.updateQuotes();
    for (Size i = 0; i < td.interpolationTimes.size(); i++) {
        BOOST_CHECK_CLOSE(td.baseNewInterpResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate),
                          td.tolerance);
        BOOST_CHECK_CLOSE(td.afterNewInterpResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate),
                          td.tolerance);
    }

    // Move date back to first test date
    Settings::instance().evaluationDate() = td.testDates[0];

    // Check interpolations again with the new quotes
    for (Size i = 0; i < td.interpolationTimes.size(); i++) {
        BOOST_CHECK_CLOSE(td.baseNewInterpResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate),
                          td.tolerance);
        BOOST_CHECK_CLOSE(td.baseNewInterpResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate),
                          td.tolerance);
    }
}

BOOST_AUTO_TEST_CASE(testDatesAndPricesCurve) {

    BOOST_TEST_MESSAGE("Testing interpolated price curve built from dates and prices");

    CommonData td;

    // Look at the first test date
    Settings::instance().evaluationDate() = td.testDates[0];

    // Create a linearly interpolated price curve
    vector<Date> dates = td.dates(0);
    InterpolatedPriceCurve<Linear> priceCurve(dates, td.prices, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, priceCurve, false);

    // Create a loglinearly interpolated price curve
    InterpolatedPriceCurve<LogLinear> logPriceCurve(dates, td.prices, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, logPriceCurve, true);

    // Check linearly interpolated price curve after moving reference date
    Settings::instance().evaluationDate() = td.testDates[1];

    // Check curve reference date is still first test date
    BOOST_CHECK_EQUAL(priceCurve.referenceDate(), td.testDates[0]);

    // Requesting price by time or date should give the same results as previously
    // because this is a fixed reference date curve
    for (Size i = 0; i < td.interpolationTimes.size(); i++) {
        BOOST_CHECK_CLOSE(td.baseExpInterpResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate),
                          td.tolerance);
        BOOST_CHECK_CLOSE(td.baseExpInterpResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate),
                          td.tolerance);
    }
}

BOOST_AUTO_TEST_CASE(testDatesAndQuotesCurve) {

    BOOST_TEST_MESSAGE("Testing interpolated price curve built from dates and quotes");

    CommonData td;

    // Look at the first test date
    Settings::instance().evaluationDate() = td.testDates[0];

    // Create a linearly interpolated price curve
    vector<Date> dates = td.dates(0);
    InterpolatedPriceCurve<Linear> priceCurve(dates, td.quotes, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, priceCurve, false);

    // Create a loglinearly interpolated price curve
    InterpolatedPriceCurve<LogLinear> logPriceCurve(dates, td.quotes, td.curveDayCounter);

    // Common checks on curve
    commonChecks(td, logPriceCurve, true);

    // Check linearly interpolated price curve after moving reference date
    Settings::instance().evaluationDate() = td.testDates[1];

    // Check curve reference date is still first test date
    BOOST_CHECK_EQUAL(priceCurve.referenceDate(), td.testDates[0]);

    // Requesting price by time or date should give the same results as previously
    // because this is a fixed reference date curve
    for (Size i = 0; i < td.interpolationTimes.size(); i++) {
        BOOST_CHECK_CLOSE(td.baseExpInterpResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate),
                          td.tolerance);
        BOOST_CHECK_CLOSE(td.baseExpInterpResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate),
                          td.tolerance);
    }

    // Update quotes and check the new interpolated values
    td.updateQuotes();
    for (Size i = 0; i < td.interpolationTimes.size(); i++) {
        BOOST_CHECK_CLOSE(td.baseNewInterpResults[i], priceCurve.price(td.interpolationTimes[i], td.extrapolate),
                          td.tolerance);
        BOOST_CHECK_CLOSE(td.baseNewInterpResults[i], priceCurve.price(td.interpolationDates[i], td.extrapolate),
                          td.tolerance);
    }
}

BOOST_AUTO_TEST_CASE(testNoTimeZeroWorks) {

    BOOST_TEST_MESSAGE("Test building with times without a time 0 works with extrapolation on");

    CommonData td;

    // Look at the first test date
    Settings::instance().evaluationDate() = td.testDates[0];

    // Create the price curve after removing the time 0 pillar
    vector<Time> times = td.times(0);
    times.erase(times.begin());
    td.prices.erase(td.prices.begin());
    InterpolatedPriceCurve<Linear> priceCurve(times, td.prices, td.curveDayCounter);

    // Check requests for prices between first curve time ~0.5 and 0
    BOOST_CHECK_CLOSE(15.1391304347826, priceCurve.price(0.25, td.extrapolate), td.tolerance);
    BOOST_CHECK_CLOSE(13.5521739130435, priceCurve.price(0.0, td.extrapolate), td.tolerance);

    // Test log-linear interpolation also
    InterpolatedPriceCurve<LogLinear> logPriceCurve(times, td.prices, td.curveDayCounter);
    BOOST_CHECK_CLOSE(15.331307232214800, logPriceCurve.price(0.25, td.extrapolate), td.tolerance);
    BOOST_CHECK_CLOSE(14.054688467053400, logPriceCurve.price(0.0, td.extrapolate), td.tolerance);

    // Test that an exception is thrown when extrapolation is off
    BOOST_CHECK_THROW(priceCurve.price(0.25, false), QuantLib::Error);
}

BOOST_AUTO_TEST_CASE(testNegativeTimeRequestThrows) {

    BOOST_TEST_MESSAGE("Test that requesting a price at a time before zero throws");

    CommonData td;

    // Look at the first test date
    Date today = td.testDates[0];
    Settings::instance().evaluationDate() = today;

    // Create the price curve
    vector<Time> times = td.times(0);
    InterpolatedPriceCurve<Linear> priceCurve(times, td.prices, td.curveDayCounter);

    // Check requests for prices at times < 0
    Time t = -0.5;
    Date d = today - 1 * Weeks;
    BOOST_CHECK_THROW(priceCurve.price(t, td.extrapolate), QuantLib::Error);
    BOOST_CHECK_THROW(priceCurve.price(d, td.extrapolate), QuantLib::Error);

    // Update evaluation date and request on today should throw similarly
    Settings::instance().evaluationDate() = today + 1 * Weeks;
    BOOST_CHECK_THROW(priceCurve.price(today, td.extrapolate), QuantLib::Error);
}

BOOST_AUTO_TEST_CASE(testNegativePriceThrows) {

    BOOST_TEST_MESSAGE("Test that attempting to return a negative price gives an error");

    CommonData td;

    // Look at the first test date
    Date today = td.testDates[0];
    Settings::instance().evaluationDate() = today;

    // Replace the last quote with a quote that will give a negative extrapolated price
    td.pQuotes[5]->setValue(1.0);

    // Create the price curve
    vector<Time> times = td.times(0);
    InterpolatedPriceCurve<Linear> priceCurve(times, td.quotes, td.curveDayCounter);

    // Asking for extrapolated price should give an error
    Time t = times.back() + 1.0;
    Date d = td.dates(0).back() + 1 * Years;
    BOOST_CHECK_THROW(priceCurve.price(t, td.extrapolate), QuantLib::Error);
    BOOST_CHECK_THROW(priceCurve.price(d, td.extrapolate), QuantLib::Error);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include <gflags/gflags.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random/normal_distribution.hpp>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <iostream>
#include <random>
#include <utility>
#include <vector>
#include "planner/io/tex.h"
#include "planner/planner.h"
#include "planner/pose.h"

using boost::math::constants::pi;
using boost::math::constants::two_pi;
using boost::math::constants::sixth_pi;
using boost::random::normal_distribution;

DEFINE_string(filename, "effects_of_J.tex", "name of file");
DEFINE_double(road_width, 4.0, "Road with");
DEFINE_double(cone_dist, 5.0, "Cone dist");
DEFINE_double(cone_radius, 0.15, "Cone dist");
DEFINE_double(cone_sigma, 0.5, "cone placement standart deviation");
DEFINE_double(velocity, 2, "velocity of car");
DEFINE_double(frequency, 1, "update frequency");

int main(int argc, char *argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);

  std::ofstream ofs(FLAGS_filename);
  planner::io::tex_mapper mapper(ofs);

  std::mt19937 rnd;

  std::vector<std::pair<double, double>> waypoints;
  for (double x = 0; x < 30; ++x) {
    double y = 0.8 * exp(x / 8.0);
    waypoints.push_back(std::make_pair(x, y));
    // std::cout << x << ' ' << y << std::endl;
  }
  planner::Planner plan(waypoints);

  planner::pose pos{planner::point(7, 3), 0, 10, 0};

  double road_width = FLAGS_road_width;
  double cone_radius = FLAGS_cone_radius;
  double cone_dist = FLAGS_cone_dist;
  double cone_sigma = FLAGS_cone_sigma;

  std::vector<planner::polygon> obstacles = {};

  double x_left, y_left, x_rigth, y_rigth, x1, x2, y1, y2;
  for (int j = 1; j < plan.baseframe().length() - 1; j += cone_dist) {
    auto point_1 = plan.baseframe().P(j - 0.1);
    auto point_2 = plan.baseframe().P(j + 0.1);

    Eigen::Vector2d v_1(point_1.x(), point_1.y());
    Eigen::Vector2d v_2(point_2.x(), point_2.y());

    Eigen::Vector2d n = v_2 - v_1;
    n.normalize();

    double x_left = plan.baseframe().P(j).x() + road_width * n[1];
    double y_left = plan.baseframe().P(j).y() - road_width * n[0];
    double x_rigth = plan.baseframe().P(j).x() - road_width * n[1];
    double y_rigth = plan.baseframe().P(j).y() + road_width * n[0];

    normal_distribution<> dist_x_left(x_left, cone_sigma);
    normal_distribution<> dist_y_left(y_left, cone_sigma);
    normal_distribution<> dist_x_right(x_rigth, cone_sigma);
    normal_distribution<> dist_y_right(y_rigth, cone_sigma);

    x_left = dist_x_left(rnd);
    y_left = dist_y_left(rnd);
    x_rigth = dist_x_right(rnd);
    y_rigth = dist_y_right(rnd);

    planner::polygon p1, p2;
    for (double i = 0; i < two_pi<double>(); i += sixth_pi<double>()) {
      x1 = x_left + cone_radius * cos(i);
      y1 = y_left + cone_radius * sin(i);
      x2 = x_rigth + cone_radius * cos(i);
      y2 = y_rigth + cone_radius * sin(i);
      p1.outer().push_back(planner::point(x1, y1));
      p2.outer().push_back(planner::point(x2, y2));
    }
    obstacles.push_back(p1);
    obstacles.push_back(p2);
  }

  // mapper.map(plan.baseframe());
  mapper.map(pos);

  plan.set_last_known_s(pos.s);
  plan.update(planner::point(0.0, -2.0), 0, 5, 1, obstacles);
  auto m_opt = plan.update(pos.position, pos.theta, 5, 1, obstacles);

  for (auto o : obstacles) {
    mapper.map(o);
  }

  for (auto m : plan.maneuvers()) {
    if (m.collision()) {
      mapper.map(m, "color=red");
    } else {
      mapper.map(m, "color=black");
    }
  }

  mapper.map(m_opt, "color=green");

  return 0;
}


#define BZ_NEW_EXPRESSION_TEMPLATES

#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

double myFunction(double x)
{
    return 1.0 / (1.0 + x);
}

BZ_DECLARE_FUNCTION(myFunction)

class myFunctor
{
public:
    myFunctor(double p) : period_(p) { }
    double operator()(double x) const
    { return sin(x/period_); }
    BZ_DECLARE_FUNCTOR(myFunctor)
private:
    double period_;
};

class myClass
{
public:
    myClass(double p) : power_(p) { }
    int foo(double x, double y) const
    { return int(x * pow(y,power_)); }
    BZ_DECLARE_MEMBER_FUNCTION2_RET(myClass,foo,int)
private:
    double power_;
};

int max3(int a, int b, int c) { return (a>b?(a>c?a:c):(b>c?b:c)); }
BZ_DECLARE_FUNCTION3(max3)

int main()
{
    Array<int,1> A(4), B(4), C(4), D(4);
    C = 0, 1, 2, 3;
    B = 5, 6, 7, 8;
    A = B + C;
    D = 5, 7, 9, 11;
    BZTEST(count(D==A)==4);

    A = 3 * tensor::i;
    D = 0, 3, 6, 9;
    BZTEST(count(D==A)==4);

    A = 3.0 * tensor::i;
    D = 0, 3, 6, 9;
    BZTEST(count(D==A)==4);

    A = tensor::i * 3.0;
    BZTEST(count(D==A)==4);

    A = 3;
    BZTEST(count(A==3)==4);

    A = tensor::i;
    BZTEST(count(A==C)==4);

    Array<double,1> E(4), F(4);
    E = 2.0 * sqrt(cast<double>(tensor::i * tensor::i));
    F = 2.0 * tensor::i;
    BZTEST(count(E==F)==4);

    E = sqr(tensor::i + 1.0);
    F = pow(tensor::i+1.0,2.0);
    BZTEST(count(E==F)==4);

    E = 4.0 * tensor::i;
    F = myFunction(E);
    E = 1.0 / (1.0 + 4.0 * tensor::i);
    BZTEST(count(E==F)==4);

    myFunctor f1(10.0);
    E = f1(2.5 + tensor::i);
    F = sin((2.5 + tensor::i)/10.0);
    BZTEST(count(E==F)==4);

    myClass f2(3.5);
    A = f2.foo(1.2 - tensor::i, 1.2 + tensor::i);
    B = cast<int>((1.2 - tensor::i) * pow(1.2 + tensor::i, 3.5));
    BZTEST(count(A==B)==4);

    A = 3 * tensor::i;
    B = max3(5,6,7);
    C = 9 - 2 * tensor::i;
    D = 9, 7, 7, 9;
    Array<int,1> DD(4);
    DD = max3(A,B,C);
    BZTEST(count(DD==D)==4);

}