/* -*- Mode: C++; c-basic-offset: 2; indent-tabs-mode: nil; tab-width: 8 -*- */

#include <boost/tokenizer.hpp>
#include <spotfinder/core_toolbox/distl.h>
#include <scitbx/vec3.h>

namespace af = scitbx::af;
namespace di = spotfinder::distltbx;
//this function was developed but never used; deprecate soon
Distl::spot::spot(af::flex_int::const_iterator intptr) :
  peakintensity(0.0),
  nmaxima(0),
  p_gotaxes(false)
{
  peak = Distl::point(*intptr,*(intptr+1));
  intptr+=2;
  int bodylen = *intptr; intptr++;
  bodypixels.reserve(bodylen);
  for (int i=0; i<bodylen; ++i, intptr+=2){
    bodypixels.push_back(Distl::point(*intptr,*(intptr+1)));}
}

void
di::w_spot::p_getaxes() {
//DEPRECATE SOON---DEPRECATE SOON
  if (p_gotaxes) return;
  if (bodypixels.size() < 2) {
    p_majoraxis = 1.0;
    p_minoraxis = 1.0;
    p_gotaxes = true;
    return;
  }
  // iteration to find average position
  af::tiny<double, 2> average_pixel_position = af::tiny<double, 2>(0.,0.);
  for (int i = 0; i < bodypixels.size() ; ++i) {
    average_pixel_position[0]+=bodypixels[i].x;
    average_pixel_position[1]+=bodypixels[i].y;
  }
  average_pixel_position[0]/=bodypixels.size();
  average_pixel_position[1]/=bodypixels.size();

  //iteration to find major axis
  std::vector<double> sq_distance;
  for (int i = 0; i < bodypixels.size() ; ++i) {
    sq_distance.push_back(
     (bodypixels[i].x - average_pixel_position[0]) *
     (bodypixels[i].x - average_pixel_position[0]) +
     (bodypixels[i].y - average_pixel_position[1]) *
     (bodypixels[i].y - average_pixel_position[1]));
  }
  std::vector<double>::const_iterator ptrmajor =
    std::max_element(sq_distance.begin(),sq_distance.end());
  double c_majoraxis = 2.0*std::sqrt(*ptrmajor);
  size_t posmajor = ptrmajor - sq_distance.begin();
  //printf("posmajor %7d\n",posmajor);

  //double check to verify the major axis vector:
  double dx = bodypixels[posmajor].x - average_pixel_position[0];
  double dy = bodypixels[posmajor].y - average_pixel_position[1];
  //double d_majoraxis = 2.0*std::sqrt( dx*dx + dy*dy );
  //printf("C_major %7.1f   D_major %7.1f\n",c_majoraxis,d_majoraxis);

  //iteration to find minor axis
  std::vector<double> abs_cross_product;
  for (int i = 0; i < bodypixels.size() ; ++i) {
    abs_cross_product.push_back( std::fabs(
       dx * (bodypixels[i].y - average_pixel_position[1]) -
       (bodypixels[i].x - average_pixel_position[0]) * dy
    ) );
  }
  std::vector<double>::const_iterator ptrminor =
    std::max_element(abs_cross_product.begin(),abs_cross_product.end());
  double c_minoraxis = 4.0*(*ptrminor)/c_majoraxis;
  //printf("C_major %7.1f   C_minor %7.1f\n",c_majoraxis,c_minoraxis);

  if (c_majoraxis >= c_minoraxis) {
    p_majoraxis = c_majoraxis;
    p_minoraxis = c_minoraxis;
  } else {
    p_majoraxis = c_minoraxis;
    p_minoraxis = c_majoraxis;
  }

  p_gotaxes=true;
}

// major and minor axes given in units of pixels
double
di::w_spot::get_majoraxis() {
  p_getaxes();
  return p_majoraxis;
}

double
di::w_spot::get_minoraxis() {
  p_getaxes();
  return p_minoraxis;
}

void di::w_spot::setstate(Distl::point const& pk){
  nmaxima=0;
  p_gotaxes=false;
  peak=pk;
}

/*
 * Does not take any charge sharing into account.  beam_center_x and
 * beam_center_y must be given in pixel units.
 */
scitbx::vec2<double>
di::w_spot::get_radial_and_azimuthal_size(
  double beam_center_x, double beam_center_y)
{
  if (bodypixels.size() == 0)
    return (scitbx::vec2<double>(0, 0));

  /*
   * Calculate unit-length radial and azimuthal (tangential) direction
   * vectors, r and a, respectively.  The azimuthal direction vector
   * is the radial vector rotated by 90 degrees counter-clockwise.
   */
  const scitbx::vec2<double> center(model_center());
  scitbx::vec2<double> r(center[0] - beam_center_x,
                         center[1] - beam_center_y);
  double h(r.length());
  if (h <= 0)
    return (scitbx::vec2<double>(0, 0));
  r /= h;
  scitbx::vec2<double> a(-r[1], +r[0]);

  /*
   * Determine the extent of the spot along the radial and azimuthal
   * directions from its center.
   */
  double a_max(-std::numeric_limits<double>::infinity());
  double a_min(+std::numeric_limits<double>::infinity());
  double r_max(-std::numeric_limits<double>::infinity());
  double r_min(+std::numeric_limits<double>::infinity());
  for (point_list_t::const_iterator q = bodypixels.begin();
       q != bodypixels.end();
       q++) {
    const scitbx::vec2<double> p(q->x - center[0], q->y - center[1]);
    const double pa(p * a);
    const double pr(p * r);

    if (pa > a_max)
      a_max = pa;
    if (pa < a_min)
      a_min = pa;
    if (pr > r_max)
      r_max = pr;
    if (pr < r_min)
      r_min = pr;
  }

  return (scitbx::vec2<double>(r_max - r_min, a_max - a_min));
}

/*
Commentary on DISTL, relevant to the use of DISTL for virus images.

1) the function pxlclassify considers background boxes in sequence across
   the detector face, but then when it comes to the final edge, it
   calculates the final box boundaries differently.  This makes for clumsy
   code, in particular the pxlclassify_scanbox() is called in 4 places.
   Duplicated code, harder to maintain.  Better to change the function so
   it establishes a single loop with an adjusted-size box so it only
   needs to call scanbox once.

2) consecutively scanned boxes overlap in 2 pixels.  This was probably
   intentional to avoid edge-related discontinuties; but it also has an
   unwanted consequence.  The pixelintensity[x][y] value is both used and
   modified by pxlclassify_scanbox().  Therefore, for the two pixels that
   overlap with the previous box, we are using
   pixelintensity[x][y] values modified in this cycle, while all other pixels
   have values from the last cycle.  It would be cleaner to implement boxes
   without overlaps.

3) on the same topic, it would help avoid discontinuties to determine a
   background plane using International Tables, volume F, p. 213.

4) the as-published gamma-I values don't really make sense:
   a) in cycle 1, all initial pixelintensity[x][y] values are supposed to
      be zero (but see 2 above) so it doesn't mean anything to have a
      bgupperint for this cycle.
   b) in cycle 2, we remove some box pixels from the background calculation.
      Then in cycle 3, the published procedure increases the bgupperint
      cutoff and recalculates the mask, having the effect of adding some
      pixels back to the background.  But this seems wrong; once a pixel
      is removed from background (because it may have signal in it) it
      probably shouldn't be added back.  So the bgupperint cutoff should
      remain constant across cycles.

5) regarding #3, for the HK97 image (with many spots) we are spending an
   enormous amount of time increasing the background-box size and retesting
   it to attain 2/3 of spots in background.  It is possible that by
   applying the background plane correction, we will attain the 2/3 threshhold
   much sooner.  No--just a modest improvement when the plane correction is
   applied to the pixelvalue[x][y].  Second try: the plane correction must
   also be applied when calculating the boxstd(); otherwise the estimate
   of standard deviation will be generally too high.  No:  no improvement in
   the occurrence of background-box increases (even though the sd is lower).
   There are dramatically more spots chosen around the water ring.

   Actions:
   -add a DetectorImageBase.debug_write() command to the iotbx.
   -add a w_Distl.mod_data() function to get a custom-written modified dataarray

6) Conclusions:
   - the get_underload() function is at fault; it is choosing a lower cutoff
   of 4035, which is much too high for reasonable interpretation of the
   virus diffraction.  Most true background is masked out.
   Change the code so it does a sanity check, and never masks out more than
   10% of the pixels as underloads (and thus ~90% of pixels are potential
   background pixels).

7) tackle some performance issues.  the function spotlist_to_flexint() takes
   as much CPU as the libdistl calls.  Solution: alleviate by coding in C++.

8) In tnear, there are numerous boost.python function calls for each spot,
   allowing for spot filtering.  This becomes prohibitive for large unit
   cells.  Solution: recode spot filtering tests in C++ code.
   Also: implement the SpotManager to give a more general way of subsetting
   the spot list.

   Tuesday goals:
     tweak parameters for this image--bg cutoff to 1.5, d1 to 2.5
       & see where I am.
     still don't understand why the spots bleed into each other when bg
       cutoff is at 1.5.  I thought the criteria for growing spots had to
       do with d1, not bg2.
*/

di::w_Distl::w_Distl(std::string optionstring, bool report_overloads){
/*
"       -s2     Smallest acceptible spot area. Spots with area smaller than this value are ignored.\n\n"
"               Recommended values: [3,5]\n\n"
"       -s3     Spot base area.\n\n"
"               Summary of spot shape, strength, etc. are based on spots no smaller than this size.\n\n"
"       -s7     Spot area upper bound factor. If area exceeds\n"
"                   median + (95th prctile - 5th prctile) * factor,\n"
"               the spot is eliminated.\n\n"
"               Recommended value: [2, 5]\n\n"
"       -s8     Spot peak intensity upper bound factor. If peak intensity exceeds \n"
"                   median + (95th prctile - 5th prctile) * factor,\n"
"               the spot is eliminated.\n\n"
"               Recommended value: [2, 10]\n\n"
"   Ice-Ring Detection Parameters:\n\n"
"       -i3     Intensity percentile as a measure of ice-ring strength.\n\n"
"                               Recommended value: [0.1, 0.3]\n\n"
        -d1     Diffraction lower intensity; a lower bound for finding maxima, default 3.5
    More parameters added Feb. 2006 for virus work
        -bx0,1,2 Scanboxsize integer values for cycles 1,2,and 3.
        -bg0,1,2 Bgupperint cutoff values for cycles 1,2,and 3.
*/
    finder.spotbasesize = 10; // distl initializes it as 16, but the LABELIT default is 10.
    finder.bgupperint[0] = 1.5; //See note 4a above; value shouldn't be used
    finder.bgupperint[1] = 1.5; //See note 4b above
    finder.bgupperint[2] = 1.5; //See note 4b above
    if (report_overloads) {finder.report_overloads = true;}
    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep(" ");
    tokenizer tok(optionstring,sep);
    for (tokenizer::iterator tok_iter = tok.begin();
         tok_iter!=tok.end(); ++tok_iter){
      if        (*tok_iter == "-s2") {
        finder.spotarealowcut = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-s3") {
        finder.spotbasesize = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-s7") {
        finder.spotareamaxfactor = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-s8") {
        finder.spotpeakintmaxfactor = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-i3") {
        finder.icering_strengthprctile = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-d1") {
        finder.difflowerint = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bx0") {
        finder.scanboxsize[0] = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bx1") {
        finder.scanboxsize[1] = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bx2") {
        finder.scanboxsize[2] = atoi((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bg0") {
        finder.bgupperint[0] = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bg1") {
        finder.bgupperint[1] = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-bg2") {
        finder.bgupperint[2] = atof((*(++tok_iter)).c_str());
      } else if (*tok_iter == "-ro") {
        set_resolution_outer( atof((*(++tok_iter)).c_str()) );
      }
    }
}

void
di::w_Distl::set_resolution_outer(const double& newvalue)
{
  // only meaningful if the resolution value is a positive number.
  SCITBX_ASSERT(newvalue>0.0);
  finder.resolution_outer = newvalue;
  //SCITBX_EXAMINE(finder.resolution_outer);
}

void
di::w_Distl::setspotimg(const double& pixel_size,
                        const double& distance,
                        const double& wavelength,
                        const double& beamx,
                        const double& beamy,
                        af::flex_int const& intdata,
                        const int& peripheral_margin,
                        const double& saturation )
{
  finder.overloadvalue = saturation;
  //Take default angle settings because we don't use them anyway.
  finder.set_imageheader(pixel_size, distance, wavelength, 0., 0.,
                         beamx, beamy);

  int ncols = intdata.accessor().all()[0];
  int nrows = intdata.accessor().all()[1];

  finder.imgmargin = peripheral_margin;
  finder.set_imagedata(intdata.begin(),ncols,nrows);
}

void
di::w_Distl::set_tiling(const string& vendortype)
{
  if (vendortype=="Pilatus-6M") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_pilatus6M(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  } else if (vendortype=="Pilatus-2M") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_pilatus2M(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  } else if (vendortype=="Pilatus-300K") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_pilatus300K(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  } else if (vendortype.substr(0,5)=="Eiger") {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_eiger(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty,
      finder.pixelvalue.nx, finder.pixelvalue.ny));
  } else {
    finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling(
      finder.firstx, finder.lastx, finder.firsty, finder.lasty));
  }
}

void
di::w_Distl::set_tiling(af::flex_int const& explicit_tiling,int const& peripheral_margin)
{
  finder.tiling = Distl::ptr_tiling(new Distl::scanbox_tiling_explicit(
      explicit_tiling, peripheral_margin));
}

af::flex_double
di::w_Distl::Z_data()
{
  int nrows=finder.pixelvalue.ny;
  af::flex_double z(af::flex_grid<>(finder.pixelvalue.nx,nrows));

  double* begin = z.begin();

  for (int x=0; x<finder.pixelvalue.nx; x++) {
    for (int y=0; y<nrows; y++){
        //capture DISTL's Z-function;
        *begin++ = finder.pixelintensity[x][y];
    }
  }
  return z;
}

af::flex_int
di::w_Distl::mod_data()
{
  int nrows=finder.pixelvalue.ny;
  af::flex_int z(af::flex_grid<>(finder.pixelvalue.nx,nrows));

  int* begin = z.begin();

  for (int x=0; x<finder.pixelvalue.nx; x++) {
    for (int y=0; y<nrows; y++){
        //example mod_data function; pixel value conditional on some property
        if (finder.pixelintensity[x][y]<1.5 &&
        finder.pixelvalue[x][y]>finder.underloadvalue &&
        finder.pixelvalue[x][y]<finder.overloadvalue
        ){
            *begin = 10;
        }else{
            *begin = finder.pixelvalue[x][y];
        }
        begin++;
    }
  }
  return z;
}

void
di::w_Distl::finish_analysis(){

  // Now done as python calls to individual methods-->finder.process();

  //get the spots out of std::list and in to af::shared

  spots.reserve(finder.spots.size());
  list<Distl::spot>::const_iterator position = finder.spots.begin();
  list<Distl::spot>::const_iterator end = finder.spots.end();
  for (; position!=end;++position ) {

    //It turns out that minimum spot area is an extremely important
    //  filter without which all sorts of junk are reported.
    /* 12oct2011-->the following line has a bug; should be >=
     * However, can't fix the bug without changing all previous results
     * For now simply work around it by decrementing distl.minimum_spot_area by 1
     */
    if ((*position).area() > finder.spotbasesize) {
      spots.push_back(*position);
    }
  }

  icerings.reserve(finder.icerings.size());
  vector<Distl::icering>::const_iterator iposition = finder.icerings.begin();
  vector<Distl::icering>::const_iterator iend = finder.icerings.end();

  for (; iposition!=iend;++iposition ) {
    icerings.push_back(*iposition);
  }

}

bool
di::w_Distl::isIsolated(const w_spot& spot, const double& mmradius) const {
  //Future:  1) find cases that use this algorithm
  //         2) change the algorithm so it relies on elliptical modelling
  //            rather than on the borderpixels data structure
  double pixelradius = mmradius / finder.pixel_size;
  typedef scitbx::vec2<double>                 vpoint;
  af::shared<Distl::point>::const_iterator sptr;
  af::shared<Distl::point>::const_iterator send;

  //vpoint focusspot(spot.ctr_mass_x(),spot.ctr_mass_y());
  vpoint focusspot(spot.max_pxl_x(),spot.max_pxl_y());

  spot_list_t::const_iterator p = spots.begin();
  spot_list_t::const_iterator e = spots.end();
  for (; p!=e; ++p) {
      //vpoint target((*p).ctr_mass_x(),(*p).ctr_mass_y());
      vpoint target((*p).max_pxl_x(),(*p).max_pxl_y());
      vpoint diff = target - focusspot;
      double dist = std::sqrt(diff*diff);
      if ( dist > pixelradius ) {continue;}

      // Consider border pixels of the focus spot
      vpoint bisector = 0.45*diff;
      double bisectorsq = bisector*bisector;
      sptr = spot.borderpixels.begin();
      send = spot.borderpixels.end();
      for (;sptr!=send; ++sptr) {
        vpoint borderpt((*sptr).x,(*sptr).y);
        vpoint bordervec = borderpt - focusspot;
        if ( bordervec*bisector > bisectorsq ) {return false;}
      }

      // Consider border pixels of the target spot
      bisector = 0.55*diff;
      bisectorsq = bisector*bisector;
      sptr = (*p).borderpixels.begin();
      send = (*p).borderpixels.end();
      for (;sptr!=send; ++sptr) {
        vpoint borderpt((*sptr).x,(*sptr).y);
        vpoint bordervec = borderpt - focusspot;
        if ( bordervec*bisector < bisectorsq ) {return false;}
      }
  }
  return true;
}


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

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008 Ralph Schreyer
 Copyright (C) 2008, 2009, 2010, 2015 Klaus Spanderen

 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 "fdmlinearop.hpp"
#include "utilities.hpp"

#include <ql/quotes/simplequote.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>
#include <ql/processes/hestonprocess.hpp>
#include <ql/processes/hullwhiteprocess.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/processes/hybridhestonhullwhiteprocess.hpp>
#include <ql/experimental/math/numericaldifferentiation.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/bicubicsplineinterpolation.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/math/integrals/discreteintegrals.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/models/equity/hestonmodel.hpp>
#include <ql/termstructures/yield/zerocurve.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/pricingengines/vanilla/mchestonhullwhiteengine.hpp>
#include <ql/methods/finitedifferences/finitedifferencemodel.hpp>
#include <ql/math/matrixutilities/gmres.hpp>
#include <ql/math/matrixutilities/bicgstab.hpp>
#include <ql/methods/finitedifferences/schemes/douglasscheme.hpp>
#include <ql/methods/finitedifferences/schemes/hundsdorferscheme.hpp>
#include <ql/methods/finitedifferences/schemes/craigsneydscheme.hpp>
#include <ql/methods/finitedifferences/meshers/uniformgridmesher.hpp>
#include <ql/methods/finitedifferences/meshers/uniform1dmesher.hpp>
#include <ql/methods/finitedifferences/meshers/concentrating1dmesher.hpp>
#include <ql/methods/finitedifferences/meshers/fdmblackscholesmesher.hpp>
#include <ql/methods/finitedifferences/solvers/fdmbackwardsolver.hpp>
#include <ql/methods/finitedifferences/operators/fdmblackscholesop.hpp>
#include <ql/methods/finitedifferences/utilities/fdmmesherintegral.hpp>
#include <ql/methods/finitedifferences/utilities/fdminnervaluecalculator.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearop.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearoplayout.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearopcomposite.hpp>
#include <ql/methods/finitedifferences/operators/fdmhestonhullwhiteop.hpp>
#include <ql/methods/finitedifferences/meshers/fdmhestonvariancemesher.hpp>
#include <ql/methods/finitedifferences/operators/fdmhestonop.hpp>
#include <ql/methods/finitedifferences/solvers/fdmhestonsolver.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmeshercomposite.hpp>
#include <ql/methods/finitedifferences/solvers/fdmndimsolver.hpp>
#include <ql/methods/finitedifferences/solvers/fdm3dimsolver.hpp>
#include <ql/methods/finitedifferences/stepconditions/fdmamericanstepcondition.hpp>
#include <ql/methods/finitedifferences/stepconditions/fdmstepconditioncomposite.hpp>
#include <ql/methods/finitedifferences/utilities/fdmdividendhandler.hpp>
#include <ql/methods/finitedifferences/operators/firstderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/secondderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/secondordermixedderivativeop.hpp>
#include <ql/math/matrixutilities/sparseilupreconditioner.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#if !defined(QL_NO_UBLAS_SUPPORT)
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#endif
#include <boost/bind.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif
#include <numeric>

using namespace QuantLib;
using namespace boost::unit_test_framework;

namespace {

    class FdmHestonExpressCondition : public StepCondition<Array> {
      public:
        FdmHestonExpressCondition(
            const std::vector<Real>& redemptions,
            const std::vector<Real>& triggerLevels,
            const std::vector<Time>& exerciseTimes,
            const boost::shared_ptr<FdmMesher> & mesher)
        : redemptions_(redemptions), triggerLevels_(triggerLevels),
          exerciseTimes_(exerciseTimes), mesher_(mesher) {
        }

        void applyTo(Array& a, Time t) const {
            std::vector<Time>::const_iterator iter
                = std::find(exerciseTimes_.begin(), exerciseTimes_.end(), t);

            if (iter != exerciseTimes_.end()) {
                Size index = std::distance(exerciseTimes_.begin(), iter);

               boost::shared_ptr<FdmLinearOpLayout> layout = mesher_->layout();
                const FdmLinearOpIterator endIter = layout->end();
                for (FdmLinearOpIterator iter = layout->begin();
                     iter != endIter; ++iter) {
                    const Real s = std::exp(mesher_->location(iter, 0));

                    if (s > triggerLevels_[index]) {
                        a[iter.index()] = redemptions_[index];
                    }
                }
            }
        }

      private:
        const std::vector<Real> redemptions_;
        const std::vector<Real> triggerLevels_;
        const std::vector<Time> exerciseTimes_;
        const boost::shared_ptr<FdmMesher> mesher_;
    };

    class ExpressPayoff : public Payoff {
      public:
        std::string name() const { return "ExpressPayoff";}
        std::string description() const { return "ExpressPayoff";}

        Real operator()(Real s) const {
            return  ((s >= 100.0) ? 108.0 : 100.0)
                  - ((s <= 75.0) ? 100.0 - s : 0.0);
        }
    };

    template <class T, class U, class V>
    struct multiplies : public std::binary_function<T, U, V> {
        V operator()(T t, U u) { return t*u;}
    };

}

void FdmLinearOpTest::testFdmLinearOpLayout() {

    BOOST_TEST_MESSAGE("Testing indexing of a linear operator...");

    Size dims[] = {5,7,8};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    FdmLinearOpLayout layout = FdmLinearOpLayout(dim);

    Size calculatedDim = layout.dim().size();
    Size expectedDim = dim.size();
    if (calculatedDim != expectedDim) {
        BOOST_ERROR("index.dimensions() should be " << expectedDim
                    << ", but is " << calculatedDim);
    }

    Size calculatedSize = layout.size();
    Size expectedSize = std::accumulate(dim.begin(), dim.end(),
                                        1, std::multiplies<Size>());

    if (calculatedSize != expectedSize) {
        BOOST_FAIL("index.size() should be "
                    << expectedSize << ", but is " << calculatedSize);
    }

    for (Size k=0; k < dim[0]; ++k) {
        for (Size l=0; l < dim[1]; ++l) {
            for (Size m=0; m < dim[2]; ++m) {
                std::vector<Size> tmp(3);
                tmp[0] = k; tmp[1] = l; tmp[2] = m;

                Size calculatedIndex = layout.index(tmp);
                Size expectedIndex = k + l*dim[0] + m*dim[0]*dim[1];

                if (expectedIndex != layout.index(tmp)) {
                    BOOST_FAIL("index.size() should be " << expectedIndex
                                <<", but is " << calculatedIndex);
                }
            }
        }
    }

    FdmLinearOpIterator iter = layout.begin();

    for (Size m=0; m < dim[2]; ++m) {
        for (Size l=0; l < dim[1]; ++l) {
            for (Size k=0; k < dim[0]; ++k, ++iter) {
                for (Size n=1; n < 4; ++n) {
                    Size nn = layout.neighbourhood(iter, 1, n);
                    Size calculatedIndex = k + m*dim[0]*dim[1]
                       + ((l < dim[1]-n)? l+n
                                        : dim[1]-1-(l+n-(dim[1]-1)))*dim[0];

                    if (nn != calculatedIndex) {
                        BOOST_FAIL("next neighbourhood index is " << nn
                                    << " but should be " << calculatedIndex);
                    }
                }

                for (Size n=1; n < 7; ++n) {
                    Size nn = layout.neighbourhood(iter, 2, -Integer(n));
                    Size calculatedIndex = k + l*dim[0]
                            + ((m < n) ? n-m : m-n)*dim[0]*dim[1];
                    if (nn != calculatedIndex) {
                        BOOST_FAIL("next neighbourhood index is " << nn
                            << " but should be " << calculatedIndex);
                    }
                }
            }
        }
    }
}

void FdmLinearOpTest::testUniformGridMesher() {

    BOOST_TEST_MESSAGE("Testing uniform grid mesher...");

    Size dims[] = {5,7,8};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> layout(new FdmLinearOpLayout(dim));
    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>(-5, 10));
    boundaries.push_back(std::pair<Real, Real>( 5, 100));
    boundaries.push_back(std::pair<Real, Real>( 10, 20));

    UniformGridMesher mesher(layout, boundaries);

    const Real dx1 = 15.0/(dim[0]-1);
    const Real dx2 = 95.0/(dim[1]-1);
    const Real dx3 = 10.0/(dim[2]-1);

    Real tol = 100*QL_EPSILON;
    if (   std::fabs(dx1-mesher.dminus(layout->begin(),0)) > tol
        || std::fabs(dx1-mesher.dplus(layout->begin(),0)) > tol
        || std::fabs(dx2-mesher.dminus(layout->begin(),1)) > tol
        || std::fabs(dx2-mesher.dplus(layout->begin(),1)) > tol
        || std::fabs(dx3-mesher.dminus(layout->begin(),2)) > tol
        || std::fabs(dx3-mesher.dplus(layout->begin(),2)) > tol ) {
        BOOST_FAIL("inconsistent uniform mesher object");
    }
}

void FdmLinearOpTest::testFirstDerivativesMapApply() {

    BOOST_TEST_MESSAGE("Testing application of first-derivatives map...");

    Size dims[] = {400, 100, 50};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> index(new FdmLinearOpLayout(dim));

    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>(-5, 5));
    boundaries.push_back(std::pair<Real, Real>( 0, 10));
    boundaries.push_back(std::pair<Real, Real>( 5, 15));

    boost::shared_ptr<FdmMesher> mesher(
                                 new UniformGridMesher(index, boundaries));

    FirstDerivativeOp map(2, mesher);

    Array r(mesher->layout()->size());
    const FdmLinearOpIterator endIter = index->end();

    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        r[iter.index()] =  std::sin(mesher->location(iter, 0))
                         + std::cos(mesher->location(iter, 2));
    }

    Array t = map.apply(r);
    const Real dz = (boundaries[2].second-boundaries[2].first)/(dims[2]-1);
    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Size z = iter.coordinates()[2];

        const Size z0 = (z > 0) ? z-1 : 1;
        const Size z2 = (z < dims[2]-1) ? z+1 : dims[2]-2;
        const Real lz0 = boundaries[2].first + z0*dz;
        const Real lz2 = boundaries[2].first + z2*dz;

        Real expected;
        if (z == 0) {
            expected = (std::cos(boundaries[2].first+dz)
                        - std::cos(boundaries[2].first))/dz;
        }
        else if (z == dim[2]-1) {
            expected = (std::cos(boundaries[2].second)
                        - std::cos(boundaries[2].second-dz))/dz;
        }
        else {
            expected = (std::cos(lz2)-std::cos(lz0))/(2*dz);
        }

        const Real calculated = t[iter.index()];
        if (std::fabs(calculated - expected) > 1e-10) {
            BOOST_FAIL("first derivative calculation failed."
                        << "\n    calculated: " << calculated
                        << "\n    expected:   " << expected);
        }
    }

}

void FdmLinearOpTest::testSecondDerivativesMapApply() {

    BOOST_TEST_MESSAGE("Testing application of second-derivatives map...");

    Size dims[] = {50, 50, 50};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> index(new FdmLinearOpLayout(dim));

    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>( 0, 0.5));
    boundaries.push_back(std::pair<Real, Real>( 0, 0.5));
    boundaries.push_back(std::pair<Real, Real>( 0, 0.5));

    boost::shared_ptr<FdmMesher> mesher(
                            new UniformGridMesher(index, boundaries));

    Array r(mesher->layout()->size());
    const FdmLinearOpIterator endIter = index->end();

    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        r[iter.index()] = std::sin(x)*std::cos(y)*std::exp(z);
    }

    Array t = SecondDerivativeOp(0, mesher).apply(r);

    const Real tol = 5e-2;
    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Size i = iter.index();
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        Real d = -std::sin(x)*std::cos(y)*std::exp(z);
        if (iter.coordinates()[0] == 0 || iter.coordinates()[0] == dims[0]-1) {
            d = 0;
        }

        if (std::fabs(d - t[i]) > tol) {
            BOOST_FAIL("numerical derivative in dx^2 deviation is too big"
                << "\n  found at " << x << " " << y << " " << z);
        }
    }

    t = SecondDerivativeOp(1, mesher).apply(r);
    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Size i = iter.index();
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        Real d = -std::sin(x)*std::cos(y)*std::exp(z);
        if (iter.coordinates()[1] == 0 || iter.coordinates()[1] == dims[1]-1) {
            d = 0;
        }

        if (std::fabs(d - t[i]) > tol) {
            BOOST_FAIL("numerical derivative in dy^2 deviation is too big"
                << "\n  found at " << x << " " << y << " " << z);
        }
    }

    t = SecondDerivativeOp(2, mesher).apply(r);
    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Size i = iter.index();
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        Real d = std::sin(x)*std::cos(y)*std::exp(z);
        if (iter.coordinates()[2] == 0 || iter.coordinates()[2] == dims[2]-1) {
            d = 0;
        }

        if (std::fabs(d - t[i]) > tol) {
            BOOST_FAIL("numerical derivative in dz^2 deviation is too big"
                << "\n  found at " << x << " " << y << " " << z);
        }
    }

}

void FdmLinearOpTest::testDerivativeWeightsOnNonUniformGrids() {
#ifndef QL_NO_UBLAS_SUPPORT
    BOOST_TEST_MESSAGE("Testing finite differences coefficients...");

    const boost::shared_ptr<Fdm1dMesher> mesherX(
        new Concentrating1dMesher(-2.0, 3.0, 50, std::make_pair(0.5, 0.01)));
    const boost::shared_ptr<Fdm1dMesher> mesherY(
        new Concentrating1dMesher(0.5, 5.0, 25, std::make_pair(0.5, 0.1)));
    const boost::shared_ptr<Fdm1dMesher> mesherZ(
        new Concentrating1dMesher(-1.0, 2.0, 31, std::make_pair(1.5, 0.01)));

    const boost::shared_ptr<FdmMesher> meshers(
        new FdmMesherComposite(mesherX, mesherY, mesherZ));

    const boost::shared_ptr<FdmLinearOpLayout> layout = meshers->layout();
    const FdmLinearOpIterator endIter = layout->end();

    const Real tol = 1e-13;
    for (Size direction=0; direction < 3; ++direction) {

        const SparseMatrix dfdx
            = FirstDerivativeOp(direction, meshers).toMatrix();
        const SparseMatrix d2fdx2
            = SecondDerivativeOp(direction, meshers).toMatrix();

        const Array gridPoints = meshers->locations(direction);

        for (FdmLinearOpIterator iter=layout->begin();
            iter != endIter; ++iter) {

            const Size c = iter.coordinates()[direction];
            const Size index   = iter.index();
            const Size indexM1 = layout->neighbourhood(iter,direction,-1);
            const Size indexP1 = layout->neighbourhood(iter,direction,+1);

            // test only if not on the boundary
            if (c == 0) {
                Array twoPoints(2);
                twoPoints[0] = 0.0;
                twoPoints[1] = gridPoints.at(indexP1)-gridPoints.at(index);

                const Array ndWeights1st = NumericalDifferentiation(
                    boost::function<Real(Real)>(), 1 , twoPoints).weights();

                const Real beta1  = dfdx(index, index);
                const Real gamma1 = dfdx(index, indexP1);
                if (   std::fabs((beta1  - ndWeights1st.at(0))/beta1) > tol
                    || std::fabs((gamma1 - ndWeights1st.at(1))/gamma1) > tol) {
                    BOOST_FAIL("can not reproduce the weights of the "
                               "first order derivative operator "
                               "on the lower boundary"
                            << "\n expected beta:    " << ndWeights1st.at(0)
                            << "\n calculated beta:  " << beta1
                            << "\n difference beta:  "
                            << beta1 - ndWeights1st.at(0)
                            << "\n expected gamma:   " << ndWeights1st.at(1)
                            << "\n calculated gamma: " << gamma1
                            << "\n difference gamma: "
                            << gamma1 - ndWeights1st.at(1));
                }

                // free boundary condition by default
                const Real beta2  = d2fdx2(index, index);
                const Real gamma2 = d2fdx2(index, indexP1);

                if (   std::fabs(beta2)  > QL_EPSILON
                    || std::fabs(gamma2) > QL_EPSILON) {
                    BOOST_FAIL("can not reproduce the weights of the "
                               "second order derivative operator "
                               "on the lower boundary"
                            << "\n expected beta:    " << 0.0
                            << "\n calculated beta:  " << beta2
                            << "\n expected gamma:   " << 0.0
                            << "\n calculated gamma: " << gamma2);
                }
            }
            else if (c == layout->dim()[direction]-1) {
                Array twoPoints(2);
                twoPoints[0] = gridPoints.at(indexM1)-gridPoints.at(index);
                twoPoints[1] = 0.0;

                const Array ndWeights1st = NumericalDifferentiation(
                    boost::function<Real(Real)>(), 1 , twoPoints).weights();

                const Real alpha1 = dfdx(index, indexM1);
                const Real beta1  = dfdx(index, index);
                if (   std::fabs((alpha1 - ndWeights1st.at(0))/alpha1) > tol
                    || std::fabs((beta1  - ndWeights1st.at(1))/beta1) > tol) {
                    BOOST_FAIL("can not reproduce the weights of the "
                               "first order derivative operator "
                               "on the upper boundary"
                            << "\n expected alpha:   " << ndWeights1st.at(0)
                            << "\n calculated alpha: " << alpha1
                            << "\n difference alpha: "
                            << alpha1 - ndWeights1st.at(0)
                            << "\n expected beta:    " << ndWeights1st.at(1)
                            << "\n calculated beta:  " << beta1
                            << "\n difference beta:  "
                            << beta1 - ndWeights1st.at(1));
                }

                // free boundary condition by default
                const Real alpha2 = d2fdx2(index, indexM1);
                const Real beta2  = d2fdx2(index, index);

                if (   std::fabs(alpha2)  > QL_EPSILON
                    || std::fabs(beta2) > QL_EPSILON) {
                    BOOST_FAIL("can not reproduce the weights of the "
                               "second order derivative operator "
                               "on the upper boundary"
                            << "\n expected alpha:   " << 0.0
                            << "\n calculated alpha: " << alpha2
                            << "\n expected beta:    " << 0.0
                            << "\n calculated beta:  " << beta2);
                }
            }
            else {
                Array threePoints(3);
                threePoints[0] = gridPoints.at(indexM1)-gridPoints.at(index);
                threePoints[1] = 0.0;
                threePoints[2] = gridPoints.at(indexP1)-gridPoints.at(index);

                const Array ndWeights1st = NumericalDifferentiation(
                    boost::function<Real(Real)>(), 1 , threePoints).weights();

                const Real alpha1 = dfdx(index, indexM1);
                const Real beta1  = dfdx(index, index);
                const Real gamma1 = dfdx(index, indexP1);

                if (   std::fabs((alpha1 - ndWeights1st.at(0))/alpha1) > tol
                    || std::fabs((beta1  - ndWeights1st.at(1))/beta1) > tol
                    || std::fabs((gamma1 - ndWeights1st.at(2))/gamma1) > tol) {
                    BOOST_FAIL("can not reproduce the weights of the "
                               "first order derivative operator"
                            << "\n expected alpha:   " << ndWeights1st.at(0)
                            << "\n calculated alpha: " << alpha1
                            << "\n difference alpha: "
                            << alpha1 - ndWeights1st.at(0)
                            << "\n expected beta:    " << ndWeights1st.at(1)
                            << "\n calculated beta:  " << beta1
                            << "\n difference beta:  "
                            << beta1 - ndWeights1st.at(1)
                            << "\n expected gamma:   " << ndWeights1st.at(2)
                            << "\n calculated gamma: " << gamma1
                            << "\n difference gamma: "
                            << gamma1 - ndWeights1st.at(2));
                }

                const Array ndWeights2nd = NumericalDifferentiation(
                    boost::function<Real(Real)>(), 2 , threePoints).weights();

                const Real alpha2 = d2fdx2(index, indexM1);
                const Real beta2  = d2fdx2(index, index);
                const Real gamma2 = d2fdx2(index, indexP1);
                if (   std::fabs((alpha2 - ndWeights2nd.at(0))/alpha2) > tol
                    || std::fabs((beta2  - ndWeights2nd.at(1))/beta2) > tol
                    || std::fabs((gamma2 - ndWeights2nd.at(2))/gamma2) > tol) {
                    BOOST_FAIL("can not reproduce the weights of the "
                               "second order derivative operator"
                            << "\n expected alpha:   " << ndWeights2nd.at(0)
                            << "\n calculated alpha: " << alpha2
                            << "\n difference alpha: "
                            << alpha2 - ndWeights2nd.at(0)
                            << "\n expected beta:    " << ndWeights2nd.at(1)
                            << "\n calculated beta:  " << beta2
                            << "\n difference beta:  "
                            << beta2 - ndWeights2nd.at(1)
                            << "\n expected gamma:   " << ndWeights2nd.at(2)
                            << "\n calculated gamma: " << gamma2
                            << "\n difference gamma: "
                            << gamma2 - ndWeights2nd.at(2));
                }
            }
        }
    }
#endif
}

void FdmLinearOpTest::testSecondOrderMixedDerivativesMapApply() {

    BOOST_TEST_MESSAGE(
        "Testing application of second-order mixed-derivatives map...");

    Size dims[] = {50, 50, 50};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> index(new FdmLinearOpLayout(dim));

    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>( 0, 0.5));
    boundaries.push_back(std::pair<Real, Real>( 0, 0.5));
    boundaries.push_back(std::pair<Real, Real>( 0, 0.5));

    boost::shared_ptr<FdmMesher> mesher(
        new UniformGridMesher(index, boundaries));

    Array r(mesher->layout()->size());
    const FdmLinearOpIterator endIter = index->end();

    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        r[iter.index()] = std::sin(x)*std::cos(y)*std::exp(z);
    }

    Array t = SecondOrderMixedDerivativeOp(0, 1, mesher).apply(r);
    Array u = SecondOrderMixedDerivativeOp(1, 0, mesher).apply(r);

    const Real tol = 5e-2;
    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Size i = iter.index();
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        const Real d = -std::cos(x)*std::sin(y)*std::exp(z);

        if (std::fabs(d - t[i]) > tol) {
            BOOST_FAIL("numerical derivative in dxdy deviation is too big"
                        << "\n  found at " << x << " " << y << " " << z);
        }

        if (std::fabs(t[i]-u[i]) > 1e5*QL_EPSILON) {
            BOOST_FAIL("numerical derivative in dxdy not equal to dydx"
                    << "\n  found at " << x << " " << y << " " << z
                    << "\n  value    " << std::fabs(t[i]-u[i]));
        }
    }

    t = SecondOrderMixedDerivativeOp(0, 2, mesher).apply(r);
    u = SecondOrderMixedDerivativeOp(2, 0, mesher).apply(r);
    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Size i = iter.index();
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        const Real d = std::cos(x)*std::cos(y)*std::exp(z);

        if (std::fabs(d - t[i]) > tol) {
            BOOST_FAIL("numerical derivative in dxdy deviation is too big"
                << "\n  found at " << x << " " << y << " " << z);
        }

        if (std::fabs(t[i]-u[i]) > 1e5*QL_EPSILON) {
            BOOST_FAIL("numerical derivative in dxdz not equal to dzdx"
                << "\n  found at " << x << " " << y << " " << z
                << "\n  value    " << std::fabs(t[i]-u[i]));
        }
    }

    t = SecondOrderMixedDerivativeOp(1, 2, mesher).apply(r);
    u = SecondOrderMixedDerivativeOp(2, 1, mesher).apply(r);
    for (FdmLinearOpIterator iter = index->begin(); iter != endIter; ++iter) {
        const Size i = iter.index();
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        const Real d = -std::sin(x)*std::sin(y)*std::exp(z);

        if (std::fabs(d - t[i]) > tol) {
            BOOST_FAIL("numerical derivative in dydz deviation is too big"
                << "\n  found at " << x << " " << y << " " << z);
        }

        if (std::fabs(t[i]-u[i]) > 1e5*QL_EPSILON) {
            BOOST_FAIL("numerical derivative in dydz not equal to dzdy"
                << "\n  found at " << x << " " << y << " " << z
                << "\n  value    " << std::fabs(t[i]-u[i]));
        }
    }

}

void FdmLinearOpTest::testTripleBandMapSolve() {

    BOOST_TEST_MESSAGE("Testing triple-band map solution...");

    Size dims[] = {100, 400};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> layout(new FdmLinearOpLayout(dim));

    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>( 0, 1.0));
    boundaries.push_back(std::pair<Real, Real>( 0, 1.0));

    boost::shared_ptr<FdmMesher> mesher(
        new UniformGridMesher(layout, boundaries));

    FirstDerivativeOp dy(1, mesher);
    dy.axpyb(Array(1, 2.0), dy, dy, Array(1, 1.0));

    // check copy constructor
    FirstDerivativeOp copyOfDy(dy);

    Array u(layout->size());
    for (Size i=0; i < layout->size(); ++i)
        u[i] = std::sin(0.1*i)+std::cos(0.35*i);

    Array t(dy.solve_splitting(copyOfDy.apply(u), 1.0, 0.0));
    for (Size i=0; i < u.size(); ++i) {
        if (std::fabs(u[i] - t[i]) > 1e-6) {
            BOOST_FAIL("solve and apply are not consistent "
                    << "\n expected      : " << u[i]
                    << "\n calculated    : " << t[i]);
        }
    }

    FirstDerivativeOp dx(0, mesher);
    dx.axpyb(Array(), dx, dx, Array(1, 1.0));

    FirstDerivativeOp copyOfDx(0, mesher);
    // check assignment
    copyOfDx = dx;

    t = dx.solve_splitting(copyOfDx.apply(u), 1.0, 0.0);
    for (Size i=0; i < u.size(); ++i) {
        if (std::fabs(u[i] - t[i]) > 1e-6) {
            BOOST_FAIL("solve and apply are not consistent "
                << "\n expected      : " << u[i]
                << "\n calculated    : " << t[i]);
        }
    }

    SecondDerivativeOp dxx(0, mesher);
    dxx.axpyb(Array(1, 0.5), dxx, dx, Array(1, 1.0));

    // check of copy constructor
    SecondDerivativeOp copyOfDxx(dxx);

    t = dxx.solve_splitting(copyOfDxx.apply(u), 1.0, 0.0);

    for (Size i=0; i < u.size(); ++i) {
        if (std::fabs(u[i] - t[i]) > 1e-6) {
            BOOST_FAIL("solve and apply are not consistent "
                << "\n expected      : " << u[i]
                << "\n calculated    : " << t[i]);
        }
    }

    //check assignment operator
    copyOfDxx.add(SecondDerivativeOp(1, mesher));
    copyOfDxx = dxx;

    t = dxx.solve_splitting(copyOfDxx.apply(u), 1.0, 0.0);

    for (Size i=0; i < u.size(); ++i) {
        if (std::fabs(u[i] - t[i]) > 1e-6) {
            BOOST_FAIL("solve and apply are not consistent "
                << "\n expected      : " << u[i]
                << "\n calculated    : " << t[i]);
        }
    }
}

void FdmLinearOpTest::testFdmHestonBarrier() {

    BOOST_TEST_MESSAGE("Testing FDM with barrier option in Heston model...");

    SavedSettings backup;

    Size dims[] = {200, 100};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> index(new FdmLinearOpLayout(dim));

    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>( 3.8, 4.905274778));
    boundaries.push_back(std::pair<Real, Real>( 0.000, 1.0));

    boost::shared_ptr<FdmMesher> mesher(
        new UniformGridMesher(index, boundaries));

    Handle<Quote> s0(boost::shared_ptr<Quote>(new SimpleQuote(100.0)));

    Handle<YieldTermStructure> rTS(flatRate(0.05, Actual365Fixed()));
    Handle<YieldTermStructure> qTS(flatRate(0.0 , Actual365Fixed()));

    boost::shared_ptr<HestonProcess> hestonProcess(
        new HestonProcess(rTS, qTS, s0, 0.04, 2.5, 0.04, 0.66, -0.8));

    Settings::instance().evaluationDate() = Date(28, March, 2004);
    Date exerciseDate(28, March, 2005);

    boost::shared_ptr<FdmLinearOpComposite> hestonOp(
                                   new FdmHestonOp(mesher, hestonProcess));

    Array rhs(mesher->layout()->size());
    const FdmLinearOpIterator endIter = mesher->layout()->end();
    for (FdmLinearOpIterator iter = mesher->layout()->begin();
         iter != endIter; ++iter) {
        rhs[iter.index()]=std::max(std::exp(mesher->location(iter,0))-100, 0.0);
    }

    FdmBoundaryConditionSet bcSet;
    bcSet.push_back(boost::shared_ptr<FdmDirichletBoundary>(
        new FdmDirichletBoundary(mesher, 0.0, 0,
                                 FdmDirichletBoundary::Upper)));

    const Real theta=0.5+std::sqrt(3.0)/6.;
    HundsdorferScheme hsEvolver(theta, 0.5, hestonOp, bcSet);
    FiniteDifferenceModel<HundsdorferScheme> hsModel(hsEvolver);
    hsModel.rollback(rhs, 1.0, 0.0, 50);

    Matrix ret(dims[0], dims[1]);
    for (Size i=0; i < dims[0]; ++i)
        for (Size j=0; j < dims[1]; ++j)
            ret[i][j] = rhs[i+j*dims[0]];

    std::vector<Real> tx, ty;
    for (FdmLinearOpIterator iter = mesher->layout()->begin();
        iter != endIter; ++iter) {
            if (iter.coordinates()[1] == 0) {
                tx.push_back(mesher->location(iter, 0));
            }
            if (iter.coordinates()[0] == 0) {
                ty.push_back(mesher->location(iter, 1));
            }
    }

    BilinearInterpolation interpolate(ty.begin(), ty.end(),
                                      tx.begin(), tx.end(), ret);

    const Real x = 100;
    const Real v0 = 0.04;

    const Real npv = interpolate(v0, std::log(x));
    const Real delta = 0.5*(
            interpolate(v0, std::log(x+1))-interpolate(v0, std::log(x-1)));
    const Real gamma =   interpolate(v0, std::log(x+1))
                      +  interpolate(v0, std::log(x-1)) - 2*npv;

    const Real npvExpected   = 9.049016;
    const Real deltaExpected = 0.511285;
    const Real gammaExpected = -0.034296;

    if (std::fabs(npv - npvExpected) > 0.000001) {
        BOOST_FAIL("Error in calculating PV for Heston barrier option");
    }

    if (std::fabs(delta - deltaExpected) > 0.000001) {
        BOOST_FAIL("Error in calculating Delta for Heston barrier option");
    }

    if (std::fabs(gamma - gammaExpected) > 0.000001) {
            BOOST_FAIL("Error in calculating Gamma for Heston barrier option");
    }
}

void FdmLinearOpTest::testFdmHestonAmerican() {

    BOOST_TEST_MESSAGE("Testing FDM with American option in Heston model...");

    SavedSettings backup;

    Size dims[] = {200, 100};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> index(new FdmLinearOpLayout(dim));

    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>( 3.8, std::log(220.0)));
    boundaries.push_back(std::pair<Real, Real>( 0.000, 1.0));

    boost::shared_ptr<FdmMesher> mesher(
        new UniformGridMesher(index, boundaries));

    Handle<Quote> s0(boost::shared_ptr<Quote>(new SimpleQuote(100.0)));

    Handle<YieldTermStructure> rTS(flatRate(0.05, Actual365Fixed()));
    Handle<YieldTermStructure> qTS(flatRate(0.0 , Actual365Fixed()));

    boost::shared_ptr<HestonProcess> hestonProcess(
        new HestonProcess(rTS, qTS, s0, 0.04, 2.5, 0.04, 0.66, -0.8));

    Settings::instance().evaluationDate() = Date(28, March, 2004);
    Date exerciseDate(28, March, 2005);

    boost::shared_ptr<FdmLinearOpComposite> LinearOp(
        new FdmHestonOp(mesher, hestonProcess));

    boost::shared_ptr<Payoff> payoff(new PlainVanillaPayoff(Option::Put, 100.0));
    Array rhs(mesher->layout()->size());
    const FdmLinearOpIterator endIter = mesher->layout()->end();
    for (FdmLinearOpIterator iter = mesher->layout()->begin();
        iter != endIter; ++iter) {
            rhs[iter.index()]
                = payoff->operator ()(std::exp(mesher->location(iter, 0)));
    }

    FdmAmericanStepCondition condition(mesher,
        boost::shared_ptr<FdmInnerValueCalculator>(
                                     new FdmLogInnerValue(payoff, mesher, 0)));
    const Real theta=0.5+std::sqrt(3.0)/6.;
    HundsdorferScheme hsEvolver(theta, 0.5, LinearOp);
    FiniteDifferenceModel<HundsdorferScheme> hsModel(hsEvolver);
    hsModel.rollback(rhs, 1.0, 0.0, 50, condition);

    Matrix ret(dims[0], dims[1]);
    for (Size i=0; i < dims[0]; ++i)
        for (Size j=0; j < dims[1]; ++j)
            ret[i][j] = rhs[i+j*dims[0]];

    std::vector<Real> tx, ty;
    for (FdmLinearOpIterator iter = mesher->layout()->begin();
        iter != endIter; ++iter) {
            if (iter.coordinates()[1] == 0) {
                tx.push_back(mesher->location(iter, 0));
            }
            if (iter.coordinates()[0] == 0) {
                ty.push_back(mesher->location(iter, 1));
            }
    }

    BilinearInterpolation interpolate(ty.begin(), ty.end(),
                                      tx.begin(), tx.end(), ret);

    const Real x = 100;
    const Real v0 = 0.04;

    const Real npv = interpolate(v0, std::log(x));
    const Real npvExpected = 5.641648;

    if (std::fabs(npv - npvExpected) > 0.000001) {
        BOOST_FAIL("Error in calculating PV for Heston American Option");
    }
}

void FdmLinearOpTest::testFdmHestonExpress() {

    BOOST_TEST_MESSAGE("Testing FDM with express certificate in Heston model...");

    SavedSettings backup;

    Size dims[] = {200, 100};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<FdmLinearOpLayout> index(new FdmLinearOpLayout(dim));

    std::vector<std::pair<Real, Real> > boundaries;
    boundaries.push_back(std::pair<Real, Real>(3.8, std::log(220.0)));
    boundaries.push_back(std::pair<Real, Real>(0.000, 1.0));

    boost::shared_ptr<FdmMesher> mesher(
                            new UniformGridMesher(index, boundaries));

    Handle<Quote> s0(boost::shared_ptr<Quote>(new SimpleQuote(100.0)));

    Handle<YieldTermStructure> rTS(flatRate(0.05, Actual365Fixed()));
    Handle<YieldTermStructure> qTS(flatRate(0.0 , Actual365Fixed()));

    Handle<HestonProcess> hestonProcess(boost::shared_ptr<HestonProcess> (
        new HestonProcess(rTS, qTS, s0, 0.04, 2.5, 0.04, 0.66, -0.8)));

    const Date exerciseDate(28, March, 2005);
    const Date evaluationDate(28, March, 2004);
    Settings::instance().evaluationDate() = evaluationDate;

    std::vector<Real> triggerLevels(2);
    triggerLevels[0] = triggerLevels[1] = 100.0;
    std::vector<Real> redemptions(2);
    redemptions[0] = redemptions[1] = 108.0;
    std::vector<Time> exerciseTimes(2);
    exerciseTimes[0] = 0.333; exerciseTimes[1] = 0.666;

    DividendSchedule dividendSchedule(1, boost::shared_ptr<Dividend>(
        new FixedDividend(2.5, evaluationDate + Period(6, Months))));
    boost::shared_ptr<FdmDividendHandler> dividendCondition(
        new FdmDividendHandler(dividendSchedule, mesher,
                               rTS->referenceDate(),
                               rTS->dayCounter(), 0));

    boost::shared_ptr<StepCondition<Array> > expressCondition(
        new FdmHestonExpressCondition(redemptions, triggerLevels,
                                      exerciseTimes, mesher));

    std::list<std::vector<Time> > stoppingTimes;
    stoppingTimes.push_back(exerciseTimes);
    stoppingTimes.push_back(dividendCondition->dividendTimes());

    std::list<boost::shared_ptr<StepCondition<Array> > > conditions;
    conditions.push_back(expressCondition);
    conditions.push_back(dividendCondition);

    boost::shared_ptr<FdmStepConditionComposite> condition(
        new FdmStepConditionComposite(stoppingTimes, conditions));

    boost::shared_ptr<Payoff> payoff(new ExpressPayoff());

    boost::shared_ptr<FdmInnerValueCalculator> calculator(
                                    new FdmLogInnerValue(payoff, mesher, 0));

    const FdmBoundaryConditionSet bcSet;
    const FdmSolverDesc solverDesc = { mesher, bcSet,
                                       condition, calculator, 1.0, 50, 0 };
    FdmHestonSolver solver(hestonProcess, solverDesc);

    const Real s = s0->value();
    const Real v0 = 0.04;

    if (std::fabs(solver.valueAt(s, v0) - 101.027) > 0.01) {
        BOOST_FAIL("Error in calculating PV for Heston Express Certificate");
    }

    if (std::fabs(solver.deltaAt(s, v0) - 0.4181) > 0.001) {
        BOOST_FAIL("Error in calculating Delta for Heston Express Certificate");
    }

    if (std::fabs(solver.gammaAt(s, v0) + 0.0400) > 0.001) {
        BOOST_FAIL("Error in calculating Gamma for Heston Express Certificate");
    }

    if (std::fabs(solver.meanVarianceDeltaAt(s, v0) - 0.6602) > 0.001) {
        BOOST_FAIL("Error in calculating mean variance Delta for "
                "Heston Express Certificate");
    }

    if (std::fabs(solver.meanVarianceGammaAt(s, v0) + 0.0316) > 0.001) {
        BOOST_FAIL("Error in calculating mean variance Delta for "
                "Heston Express Certificate");
    }
}

namespace {

    boost::shared_ptr<HybridHestonHullWhiteProcess> createHestonHullWhite(
        Time maturity) {

        DayCounter dc = Actual365Fixed();
        const Date today = Settings::instance().evaluationDate();
        Handle<Quote> s0(boost::shared_ptr<Quote>(new SimpleQuote(100.0)));

        std::vector<Date> dates;
        std::vector<Rate> rates, divRates;

        for (Size i=0; i <= 25; ++i) {
            dates.push_back(today+Period(i, Years));
            rates.push_back(0.05);
            divRates.push_back(0.02);
        }

        const Handle<YieldTermStructure> rTS(
           boost::shared_ptr<YieldTermStructure>(new ZeroCurve(dates, rates, dc)));
        const Handle<YieldTermStructure> qTS(
           boost::shared_ptr<YieldTermStructure>(
                                              new ZeroCurve(dates, divRates, dc)));

        const Real v0 = 0.04;
        boost::shared_ptr<HestonProcess> hestonProcess(
            new HestonProcess(rTS, qTS, s0, v0, 1.0, v0*0.75, 0.4, -0.7));

        boost::shared_ptr<HullWhiteForwardProcess> hwFwdProcess(
                            new HullWhiteForwardProcess(rTS, 0.00883, 0.01));
        hwFwdProcess->setForwardMeasureTime(maturity);

        const Real equityShortRateCorr = -0.7;

        return boost::shared_ptr<HybridHestonHullWhiteProcess>(
                new HybridHestonHullWhiteProcess(hestonProcess, hwFwdProcess,
                                                 equityShortRateCorr));
    }

    FdmSolverDesc createSolverDesc(
        const std::vector<Size>& dim,
        const boost::shared_ptr<HybridHestonHullWhiteProcess>& process) {

        const Time maturity
                    = process->hullWhiteProcess()->getForwardMeasureTime();

        boost::shared_ptr<FdmLinearOpLayout> layout(new FdmLinearOpLayout(dim));

        std::vector<boost::shared_ptr<Fdm1dMesher> > mesher1d;
        mesher1d.push_back(boost::shared_ptr<Fdm1dMesher>(
                new Uniform1dMesher(std::log(22.0), std::log(440.0), dim[0])));
        mesher1d.push_back(boost::shared_ptr<Fdm1dMesher>(
                new FdmHestonVarianceMesher(dim[1], process->hestonProcess(),
                                            maturity)));
        mesher1d.push_back(boost::shared_ptr<Fdm1dMesher>(
                new Uniform1dMesher(-0.15, 0.15, dim[2])));

        const boost::shared_ptr<FdmMesher> mesher(
            new FdmMesherComposite(mesher1d));

        const FdmBoundaryConditionSet boundaries;

        std::list<std::vector<Time> > stoppingTimes;
        std::list<boost::shared_ptr<StepCondition<Array> > > stepConditions;

        boost::shared_ptr<FdmStepConditionComposite> conditions(
            new FdmStepConditionComposite(
                                     std::list<std::vector<Time> >(),
                                     FdmStepConditionComposite::Conditions()));

        boost::shared_ptr<StrikedTypePayoff> payoff(
                                  new PlainVanillaPayoff(Option::Call, 160.0));

        boost::shared_ptr<FdmInnerValueCalculator> calculator(
                                       new FdmLogInnerValue(payoff, mesher, 0));

        const Size tGrid = 100;
        const Size dampingSteps = 0;

        FdmSolverDesc desc = { mesher, boundaries,
                               conditions, calculator,
                               maturity, tGrid, dampingSteps };

        return desc;
    }
}

void FdmLinearOpTest::testFdmHestonHullWhiteOp() {
    BOOST_TEST_MESSAGE("Testing FDM with Heston Hull-White model...");

    SavedSettings backup;

    const Date today = Date(28, March, 2004);
    Settings::instance().evaluationDate() = today;

    Date exerciseDate(28, March, 2012);
    const Time maturity = Actual365Fixed().yearFraction(today, exerciseDate);

    Size dims[] = {51, 31, 31};
    const std::vector<Size> dim(dims, dims+LENGTH(dims));

    boost::shared_ptr<HybridHestonHullWhiteProcess> jointProcess
                                            = createHestonHullWhite(maturity);
    FdmSolverDesc desc = createSolverDesc(dim, jointProcess);
    boost::shared_ptr<FdmMesher> mesher = desc.mesher;

    boost::shared_ptr<HullWhiteForwardProcess> hwFwdProcess
                                            = jointProcess->hullWhiteProcess();

    boost::shared_ptr<HullWhiteProcess> hwProcess(
        new HullWhiteProcess(jointProcess->hestonProcess()->riskFreeRate(),
                             hwFwdProcess->a(), hwFwdProcess->sigma()));

    boost::shared_ptr<FdmLinearOpComposite> linearOp(
        new FdmHestonHullWhiteOp(mesher,
                                 jointProcess->hestonProcess(),
                                 hwProcess,
                                 jointProcess->eta()));

    Array rhs(mesher->layout()->size());
    const FdmLinearOpIterator endIter = mesher->layout()->end();
    for (FdmLinearOpIterator iter = mesher->layout()->begin();
        iter != endIter; ++iter) {
            rhs[iter.index()] = desc.calculator->avgInnerValue(iter, maturity);
    }

    const Real theta = 0.5+std::sqrt(3.0)/6.;
    HundsdorferScheme hsEvolver(theta, 0.5, linearOp);
    FiniteDifferenceModel<HundsdorferScheme> hsModel(hsEvolver);

    hsModel.rollback(rhs, maturity, 0.0, desc.timeSteps);

    std::vector<Real> tx, ty, tr, y;
    for (FdmLinearOpIterator iter = mesher->layout()->begin();
        iter != endIter; ++iter) {
            if (iter.coordinates()[1] == 0 && iter.coordinates()[2] == 0) {
                tx.push_back(mesher->location(iter, 0));
            }
            if (iter.coordinates()[0] == 0 && iter.coordinates()[2] == 0) {
                ty.push_back(mesher->location(iter, 1));
            }
            if (iter.coordinates()[0] == 0 && iter.coordinates()[1] == 0) {
                tr.push_back(mesher->location(iter, 2));
            }
    }

    const Real x0 = 100;
    const Real v0 = jointProcess->hestonProcess()->v0();
    const Real r0 = 0;
    for (Size k=0; k < dim[2]; ++k) {
        Matrix ret(dim[0], dim[1]);
        for (Size i=0; i < dim[0]; ++i)
            for (Size j=0; j < dim[1]; ++j)
                ret[i][j] = rhs[ i+j*dim[0]+k*dim[0]*dim[1] ];

        y.push_back(BicubicSpline(ty.begin(), ty.end(),
                                  tx.begin(), tx.end(), ret)(v0, std::log(x0)));
    }

    const Real directCalc
        = MonotonicCubicNaturalSpline(tr.begin(), tr.end(), y.begin())(r0);

    std::vector<Real> x(3);
    x[0] = std::log(x0); x[1] = v0; x[2] = r0;

    Fdm3DimSolver solver3d(desc, FdmSchemeDesc::Hundsdorfer(), linearOp);
    const Real solverCalc = solver3d.interpolateAt(x[0], x[1], x[2]);
    const Real solverTheta = solver3d.thetaAt(x[0], x[1], x[2]);

    if (std::fabs(directCalc - solverCalc) > 1e-4) {
        BOOST_FAIL("Error in calculating PV for Heston Hull White Option");
    }

    FdmNdimSolver<3> solverNd(desc, FdmSchemeDesc::Hundsdorfer(), linearOp);
    const Real solverNdCalc = solverNd.interpolateAt(x);
    const Real solverNdTheta = solverNd.thetaAt(x);

    if (std::fabs(solverNdCalc - solverCalc) > 1e-4) {
        BOOST_FAIL("Error in calculating PV for Heston Hull White Option");
    }
    if (std::fabs(solverNdTheta - solverTheta) > 1e-4) {
        BOOST_FAIL("Error in calculating PV for Heston Hull White Option");
    }

    VanillaOption option(
            boost::shared_ptr<StrikedTypePayoff>(
                                new PlainVanillaPayoff(Option::Call, 160.0)),
            boost::shared_ptr<Exercise>(new EuropeanExercise(exerciseDate)));

    const Real tol = 0.025;
    option.setPricingEngine(
            MakeMCHestonHullWhiteEngine<PseudoRandom>(jointProcess)
            .withSteps(200)
            .withAntitheticVariate()
            .withControlVariate()
            .withAbsoluteTolerance(tol)
            .withSeed(42));

    // the following takes far too long
    // const Real expected = option.NPV();
    // use precalculated value instead
    const Real expected = 4.73;

    if (std::fabs(directCalc - expected) > 3*tol) {
        BOOST_FAIL("Error in calculating MC PV for Heston Hull White Option");
    }
}

#if !defined(QL_NO_UBLAS_SUPPORT)
namespace {
    Disposable<Array> axpy(
        const boost::numeric::ublas::compressed_matrix<Real>& A,
        const Array& x) {

        boost::numeric::ublas::vector<Real> tmpX(x.size()), y(x.size());
        std::copy(x.begin(), x.end(), tmpX.begin());
        boost::numeric::ublas::axpy_prod(A, tmpX, y);

        Array retVal(y.begin(), y.end());
        return retVal;
    }

    boost::numeric::ublas::compressed_matrix<Real> createTestMatrix(
        Size n, Size m, Real theta) {

        boost::numeric::ublas::compressed_matrix<Real> a(n*m, n*m);

        for (Size i=0; i < n; ++i) {
            for (Size j=0; j < m; ++j) {
                const Size k = i*m+j;
                a(k,k)=1.0;

                if (i > 0 && j > 0 && i <n-1 && j < m-1) {
                    const Size im1 = i-1;
                    const Size ip1 = i+1;
                    const Size jm1 = j-1;
                    const Size jp1 = j+1;
                    const Real delta = theta/((ip1-im1)*(jp1-jm1));

                    a(k,im1*m+jm1) =  delta;
                    a(k,im1*m+jp1) = -delta;
                    a(k,ip1*m+jm1) = -delta;
                    a(k,ip1*m+jp1) =  delta;
                }
            }
        }

        return a;
    }
}
#endif

void FdmLinearOpTest::testBiCGstab() {
#if !defined(QL_NO_UBLAS_SUPPORT)
    BOOST_TEST_MESSAGE(
        "Testing bi-conjugated gradient stabilized algorithm...");

    const Size n=41, m=21;
    const Real theta = 1.0;
    const boost::numeric::ublas::compressed_matrix<Real> a
        = createTestMatrix(n, m, theta);

    const boost::function<Disposable<Array>(const Array&)> matmult(
                                                boost::bind(&axpy, a, _1));

    SparseILUPreconditioner ilu(a, 4);
    boost::function<Disposable<Array>(const Array&)> precond(
         boost::bind(&SparseILUPreconditioner::apply, &ilu, _1));

    Array b(n*m);
    MersenneTwisterUniformRng rng(1234);
    for (Size i=0; i < b.size(); ++i) {
        b[i] = rng.next().value;
    }

    const Real tol = 1e-10;

    const BiCGstab biCGstab(matmult, n*m, tol, precond);
    const Array x = biCGstab.solve(b).x;

    const Real error = std::sqrt(DotProduct(b-axpy(a, x),
                                 b-axpy(a, x))/DotProduct(b,b));

    if (error > tol) {
        BOOST_FAIL("Error calculating the inverse using BiCGstab" <<
                "\n tolerance:  " << tol <<
                "\n error:      " << error);
    }
#endif
}

void FdmLinearOpTest::testGMRES() {
#if !defined(QL_NO_UBLAS_SUPPORT)
    BOOST_TEST_MESSAGE("Testing GMRES algorithm...");

    const Size n=41, m=21;
    const Real theta = 1.0;
    const boost::numeric::ublas::compressed_matrix<Real> a
        = createTestMatrix(n, m, theta);

    const boost::function<Disposable<Array>(const Array&)> matmult(
                                                boost::bind(&axpy, a, _1));

    SparseILUPreconditioner ilu(a, 4);
    boost::function<Disposable<Array>(const Array&)> precond(
         boost::bind(&SparseILUPreconditioner::apply, &ilu, _1));

    Array b(n*m);
    MersenneTwisterUniformRng rng(1234);
    for (Size i=0; i < b.size(); ++i) {
        b[i] = rng.next().value;
    }

    const Real tol = 1e-10;

    const GMRES gmres(matmult, n*m, tol, precond);
    const GMRESResult result = gmres.solve(b, b);
    const Array x = result.x;
    const Real errorCalculated = result.errors.back();

    const Real error = std::sqrt(DotProduct(b-axpy(a, x),
                                 b-axpy(a, x))/DotProduct(b,b));

    if (error > tol) {
        BOOST_FAIL("Error calculating the inverse using GMRES" <<
                "\n tolerance:  " << tol <<
                "\n error:      " << error);
    }

    if (std::fabs(error - errorCalculated) > 10*QL_EPSILON) {
        BOOST_FAIL("Calculation if the error in GMRES went wrong" <<
                "\n calculated: " << errorCalculated <<
                "\n error:      " << error);

    }

    const GMRES gmresRestart(matmult, 5, tol, precond);
    const GMRESResult resultRestart = gmresRestart.solveWithRestart(5, b, b);
    const Real errorWithRestart = resultRestart.errors.back();

    if (errorWithRestart > tol) {
        BOOST_FAIL("Error calculating the inverse using "
                "GMRES with restarts" <<
                "\n tolerance:  " << tol <<
                "\n error:      " << errorWithRestart);
    }
#endif
}

void FdmLinearOpTest::testCrankNicolsonWithDamping() {

    BOOST_TEST_MESSAGE("Testing Crank-Nicolson with initial implicit damping steps "
                       "for a digital option...");

    SavedSettings backup;

    DayCounter dc = Actual360();
    Date today = Date::todaysDate();

    boost::shared_ptr<SimpleQuote> spot(new SimpleQuote(100.0));
    boost::shared_ptr<YieldTermStructure> qTS = flatRate(today, 0.06, dc);
    boost::shared_ptr<YieldTermStructure> rTS = flatRate(today, 0.06, dc);
    boost::shared_ptr<BlackVolTermStructure> volTS = flatVol(today, 0.35, dc);

    boost::shared_ptr<StrikedTypePayoff> payoff(
                             new CashOrNothingPayoff(Option::Put, 100, 10.0));

    Time maturity = 0.75;
    Date exDate = today + Integer(maturity*360+0.5);
    boost::shared_ptr<Exercise> exercise(new EuropeanExercise(exDate));

    boost::shared_ptr<BlackScholesMertonProcess> process(new
        BlackScholesMertonProcess(Handle<Quote>(spot),
                                  Handle<YieldTermStructure>(qTS),
                                  Handle<YieldTermStructure>(rTS),
                                  Handle<BlackVolTermStructure>(volTS)));
    boost::shared_ptr<PricingEngine> engine(
                                new AnalyticEuropeanEngine(process));

    VanillaOption opt(payoff, exercise);
    opt.setPricingEngine(engine);
    Real expectedPV = opt.NPV();
    Real expectedGamma = opt.gamma();

    // fd pricing using implicit damping steps and Crank Nicolson
    const Size csSteps = 25, dampingSteps = 3, xGrid = 400;
    const std::vector<Size> dim(1, xGrid);

    boost::shared_ptr<FdmLinearOpLayout> layout(new FdmLinearOpLayout(dim));
    const boost::shared_ptr<Fdm1dMesher> equityMesher(
        new FdmBlackScholesMesher(
                dim[0], process, maturity, payoff->strike(),
                Null<Real>(), Null<Real>(), 0.0001, 1.5,
                std::pair<Real, Real>(payoff->strike(), 0.01)));

    const boost::shared_ptr<FdmMesher> mesher (
        new FdmMesherComposite(equityMesher));

    boost::shared_ptr<FdmBlackScholesOp> map(
                     new FdmBlackScholesOp(mesher, process, payoff->strike()));

    boost::shared_ptr<FdmInnerValueCalculator> calculator(
                                  new FdmLogInnerValue(payoff, mesher, 0));

    Array rhs(layout->size()), x(layout->size());
    const FdmLinearOpIterator endIter = layout->end();

    for (FdmLinearOpIterator iter = layout->begin(); iter != endIter;
         ++iter) {
        rhs[iter.index()] = calculator->avgInnerValue(iter, maturity);
        x[iter.index()] = mesher->location(iter, 0);
    }

    FdmBackwardSolver solver(map, FdmBoundaryConditionSet(),
                             boost::shared_ptr<FdmStepConditionComposite>(),
                             FdmSchemeDesc::Douglas());
    solver.rollback(rhs, maturity, 0.0, csSteps, dampingSteps);

    MonotonicCubicNaturalSpline spline(x.begin(), x.end(), rhs.begin());

    Real s = spot->value();
    Real calculatedPV = spline(std::log(s));
    Real calculatedGamma = (spline.secondDerivative(std::log(s))
                            - spline.derivative(std::log(s))    )/(s*s);

    Real relTol = 2e-3;

    if (std::fabs(calculatedPV - expectedPV) > relTol*expectedPV) {
        BOOST_FAIL("Error calculating the PV of the digital option" <<
                "\n rel. tolerance:  " << relTol <<
                "\n expected:        " << expectedPV <<
                "\n calculated:      " << calculatedPV);
    }
    if (std::fabs(calculatedGamma - expectedGamma) > relTol*expectedGamma) {
        BOOST_FAIL("Error calculating the Gamma of the digital option" <<
                "\n rel. tolerance:  " << relTol <<
                "\n expected:        " << expectedGamma <<
                "\n calculated:      " << calculatedGamma);
    }
}

void FdmLinearOpTest::testSpareMatrixReference() {
#ifndef QL_NO_UBLAS_SUPPORT
    BOOST_TEST_MESSAGE("Testing SparseMatrixReference type...");

    const Size rows    = 10;
    const Size columns = 10;
    const Size nMatrices = 5;
    const Size nElements = 50;

    PseudoRandom::urng_type rng(1234ul);

    SparseMatrix expected(rows, columns);
    std::vector<SparseMatrix> v(nMatrices, SparseMatrix(rows, columns));
    std::vector<SparseMatrixReference> refs;

    for (Size i=0; i < v.size(); ++i) {
        SparseMatrixReference m(v[i]);
        for (Size j=0; j < nElements; ++j) {
            const Size row    = Size(rng.next().value*rows);
            const Size column = Size(rng.next().value*columns);

            const Real value = rng.next().value;
            m(row, column)        += value;
            expected(row, column) += value;
        }

        refs.push_back(m);
    }

    SparseMatrix calculated = std::accumulate(refs.begin()+1, refs.end(),
                                              SparseMatrix(refs.front()));

    for (Size i=0; i < rows; ++i) {
        for (Size j=0; j < columns; ++j) {
            if (std::fabs(calculated(i,j) - expected(i,j)) > 100*QL_EPSILON) {
                BOOST_FAIL("Error using sparse matrix references in " <<
                           "Element (" << i << ", " << j << ")" <<
                        "\n expected  : " << expected(i, j) <<
                        "\n calculated: " << calculated(i, j));
            }
        }
    }
#endif
}

namespace {
#ifndef QL_NO_UBLAS_SUPPORT
    Size nrElementsOfSparseMatrix(const SparseMatrix& m) {
        Size retVal = 0;
        for (SparseMatrix::const_iterator1 i1 = m.begin1();
            i1 != m.end1(); ++i1) {
            retVal+=std::distance(i1.begin(), i1.end());
        }
        return retVal;
    }
#endif
}

void FdmLinearOpTest::testSparseMatrixZeroAssignment() {
#ifndef QL_NO_UBLAS_SUPPORT
    BOOST_TEST_MESSAGE("Testing assignment to zero in sparse matrix...");

    SparseMatrix m(5,5);
    if (nrElementsOfSparseMatrix(m)) {
        BOOST_FAIL("non zero return for an emtpy matrix");
    }
    m(0, 0) = 0.0; m(1, 2) = 0.0;
    if (nrElementsOfSparseMatrix(m) != 2) {
        BOOST_FAIL("two elements expected");
    }
    m(1, 3) = 1.0;
    if (nrElementsOfSparseMatrix(m) != 3) {
        BOOST_FAIL("three elements expected");
    }
    m(1, 3) = 0.0;
    if (nrElementsOfSparseMatrix(m) != 3) {
        BOOST_FAIL("three elements expected");
    }
#endif
}

void FdmLinearOpTest::testFdmMesherIntegral() {
    BOOST_TEST_MESSAGE("Testing integrals over meshers functions...");

    const boost::shared_ptr<FdmMesherComposite> mesher(
        new FdmMesherComposite(
            boost::shared_ptr<Fdm1dMesher>(new Concentrating1dMesher(
                -1, 1.6, 21, std::pair<Real, Real>(0, 0.1))),
            boost::shared_ptr<Fdm1dMesher>(new Concentrating1dMesher(
                -3, 4, 11, std::pair<Real, Real>(1, 0.01))),
            boost::shared_ptr<Fdm1dMesher>(new Concentrating1dMesher(
                -2, 1, 5, std::pair<Real, Real>(0.5, 0.1)))));

    const boost::shared_ptr<FdmLinearOpLayout> layout = mesher->layout();

    Array f(mesher->layout()->size());
    for (FdmLinearOpIterator iter = layout->begin();
        iter != layout->end(); ++iter) {
        const Real x = mesher->location(iter, 0);
        const Real y = mesher->location(iter, 1);
        const Real z = mesher->location(iter, 2);

        f[iter.index()] = x*x + 3*y*y - 3*z*z
                        + 2*x*y - x*z - 3*y*z
                        + 4*x - y - 3*z + 2 ;
    }

    const Real tol = 1e-12;

    // Simpson's rule has to be exact here, Mathematica code gives
    // Integrate[x*x+3*y*y-3*z*z+2*x*y-x*z-3*y*z+4*x-y-3*z+2,
    //           {x, -1, 16/10}, {y, -3, 4}, {z, -2, 1}]
    const Real expectedSimpson = 876.512;
    const Real calculatedSimpson
        = FdmMesherIntegral(mesher, DiscreteSimpsonIntegral()).integrate(f);

    if (std::fabs(calculatedSimpson - expectedSimpson) > tol*expectedSimpson) {
        BOOST_FAIL(std::setprecision(16)
            << "discrete mesher integration using Simpson's rule failed: "
            << "\n    calculated: " << calculatedSimpson
            << "\n    expected:   " << expectedSimpson);
    }

    const Real expectedTrapezoid = 917.0148209153263;
    const Real calculatedTrapezoid
        = FdmMesherIntegral(mesher, DiscreteTrapezoidIntegral()).integrate(f);

    if (std::fabs(calculatedTrapezoid - expectedTrapezoid)
            > tol*expectedTrapezoid) {
        BOOST_FAIL(std::setprecision(16)
            << "discrete mesher integration using Trapezoid rule failed: "
            << "\n    calculated: " << calculatedTrapezoid
            << "\n    expected:   " << expectedTrapezoid);
    }
}

test_suite* FdmLinearOpTest::suite() {
    test_suite* suite = BOOST_TEST_SUITE("linear operator tests");

    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testFdmLinearOpLayout));
    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testUniformGridMesher));
    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testFirstDerivativesMapApply));
    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testSecondDerivativesMapApply));
    suite->add(QUANTLIB_TEST_CASE(
        &FdmLinearOpTest::testDerivativeWeightsOnNonUniformGrids));
    suite->add(QUANTLIB_TEST_CASE(
        &FdmLinearOpTest::testSecondOrderMixedDerivativesMapApply));
    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testTripleBandMapSolve));
    suite->add(QUANTLIB_TEST_CASE(&FdmLinearOpTest::testFdmHestonBarrier));
    suite->add(QUANTLIB_TEST_CASE(&FdmLinearOpTest::testFdmHestonAmerican));
    suite->add(QUANTLIB_TEST_CASE(&FdmLinearOpTest::testFdmHestonExpress));
    suite->add(QUANTLIB_TEST_CASE(&FdmLinearOpTest::testFdmHestonHullWhiteOp));
    suite->add(QUANTLIB_TEST_CASE(&FdmLinearOpTest::testBiCGstab));
    suite->add(QUANTLIB_TEST_CASE(&FdmLinearOpTest::testGMRES));
    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testCrankNicolsonWithDamping));
    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testSpareMatrixReference));
    suite->add(
        QUANTLIB_TEST_CASE(&FdmLinearOpTest::testSparseMatrixZeroAssignment));
    suite->add(QUANTLIB_TEST_CASE(&FdmLinearOpTest::testFdmMesherIntegral));

    return suite;

}


#include "pose.hpp"

#include <typeinfo> // operator typeid

#include <Eigen/Geometry>
#include <tbb/parallel_for.h>

#include <autodiff/autodiff.h>
#include <logger.hpp>
#include <profiler.hpp>
#include <utils/is_zero.hpp>
#include <utils/not_implemented_error.hpp>
#include <utils/sinc.hpp>
#include <utils/type_name.hpp>

namespace ipc::rigid {

template <typename T>
Pose<T>::Pose()
    : position()
    , rotation()
{
}

template <typename T>
Pose<T>::Pose(const VectorMax3<T>& position, const VectorMax3<T>& rotation)
    : position(position)
    , rotation(rotation)
{
}

template <typename T> Pose<T>::Pose(const VectorMax6<T>& dof)
{
    if (dof.size() == dim_to_ndof(2)) {
        position = dof.head(dim_to_pos_ndof(2));
        rotation = dof.tail(dim_to_rot_ndof(2));
    } else if (dof.size() == dim_to_ndof(3)) {
        position = dof.head(dim_to_pos_ndof(3));
        rotation = dof.tail(dim_to_rot_ndof(3));
    } else {
        throw NotImplementedError("Unknown pose convertion for given ndof");
    }
}

template <typename T>
Pose<T>::Pose(const T& x, const T& y, const T& theta)
    : Pose(Vector2<T>(x, y), Vector1<T>())
{
    rotation << theta;
}

template <typename T>
Pose<T>::Pose(
    const T& x,
    const T& y,
    const T& z,
    const T& theta_x,
    const T& theta_y,
    const T& theta_z)
    : Pose(Vector3<T>(x, y, z), Vector3<T>(theta_x, theta_y, theta_z))
{
}

template <typename T> Pose<T> Pose<T>::Zero(int dim)
{
    assert(dim == 2 || dim == 3);
    return Pose(
        VectorX<T>::Zero(Pose<T>::dim_to_pos_ndof(dim)),
        VectorX<T>::Zero(Pose<T>::dim_to_rot_ndof(dim)));
}

template <typename T>
Poses<T> Pose<T>::dofs_to_poses(const VectorX<T>& dofs, int dim)
{
    int ndof = dim_to_ndof(dim);
    int num_poses = dofs.size() / ndof;
    assert(dofs.size() % ndof == 0);
    Poses<T> poses;
    poses.reserve(num_poses);
    for (int i = 0; i < num_poses; i++) {
        poses.emplace_back(dofs.segment(i * ndof, ndof));
    }
    return poses;
}

template <typename T> VectorX<T> Pose<T>::poses_to_dofs(const Poses<T>& poses)
{
    const int ndof = poses.size() ? poses[0].ndof() : 0;
    VectorX<T> dofs(poses.size() * ndof);
    for (size_t i = 0; i < poses.size(); i++) {
        assert(poses[i].ndof() == ndof);
        dofs.segment(i * ndof, ndof) = poses[i].dof();
    }
    return dofs;
}

template <typename T> VectorMax6<T> Pose<T>::dof() const
{
    VectorMax6<T> pose_dof(ndof());
    pose_dof.head(pos_ndof()) = position;
    pose_dof.tail(rot_ndof()) = rotation;
    return pose_dof;
}

// Replace a selected dof with the dof in other.
template <typename T>
void Pose<T>::select_dof(
    const VectorMax6b& is_dof_selected,
    const Pose<T>& other,
    const MatrixMax3d& R)
{
    assert(is_dof_selected.size() == this->ndof());
    assert(other.dim() == this->dim());
    // R should be a rotation
    assert(R.isUnitary(1e-9));
    assert(fabs(R.determinant() - 1.0) < 1.0e-6);
    position =
        is_dof_selected.head(pos_ndof()).select(other.position, position);
    rotation = R.transpose()
        * is_dof_selected.tail(rot_ndof())
              .select(R * other.rotation, R * rotation);
}

// Zero out the i-th dof if is_dof_zero(i) == true.
template <typename T>
void Pose<T>::zero_dof(const VectorMax6b& is_dof_zero, const MatrixMax3d& R)
{
    select_dof(is_dof_zero, Pose<T>::Zero(dim()), R);
}

template <typename T> MatrixMax3<T> Pose<T>::construct_rotation_matrix() const
{
    return ipc::rigid::construct_rotation_matrix(rotation);
}

template <typename T> Eigen::Quaternion<T> Pose<T>::construct_quaternion() const
{
    return ipc::rigid::construct_quaternion(rotation);
}

template <typename T>
Pose<T> Pose<T>::interpolate(const Pose<T>& pose0, const Pose<T>& pose1, T t)
{
    assert(pose0.dim() == pose1.dim());
    return Pose<T>(
        (pose1.position - pose0.position) * t + pose0.position,
        (pose1.rotation - pose0.rotation) * t + pose0.rotation);
}

template <typename T> bool Pose<T>::operator==(const Pose<T>& other) const
{
    return this->position == other.position && this->rotation == other.rotation;
}

template <typename T> Pose<T>& Pose<T>::operator*=(const T& x)
{
    this->position *= x;
    this->rotation *= x;
    return *this;
}

template <typename T> Pose<T> Pose<T>::operator/(const T& x) const
{
    return Pose<T>(this->position / x, this->rotation / x);
}

///////////////////////////////////////////////////////////////////////////
// Operations on vector of Poses

template <typename T>
Poses<T> interpolate(const Poses<T>& poses0, const Poses<T>& poses1, T t)
{
    PROFILE_POINT(fmt::format("Poses<{}>::interpolate", get_type_name<T>()));
    PROFILE_START();
    Poses<T> poses(poses0.size());
    for (size_t i = 0; i < poses.size(); i++) {
        poses[i] = Pose<T>::interpolate(poses0[i], poses1[i], t);
    }
    PROFILE_END();
    return poses;
}

template <typename T> Poses<T> operator*(const Poses<T>& poses, const T& x)
{
    Poses<T> product = poses;
    for (size_t i = 0; i < product.size(); i++) {
        product[i] *= x;
    }
    return product;
}

template <typename T, typename U> Poses<T> cast(const Poses<U>& poses)
{
    Poses<T> poses_T;
    poses_T.reserve(poses_T.size());
    for (int i = 0; i < poses.size(); i++) {
        poses_T.push_back(poses[i].template cast<T>());
    }
    return poses_T;
}

template <typename T>
MatrixMax3<T> construct_rotation_matrix(const VectorMax3<T>& r)
{
    if (r.size() == 1) {
        return Eigen::Rotation2D<T>(r(0)).toRotationMatrix();
    } else {
        assert(r.size() == 3);
        T sinc_angle = sinc_normx(r);
        T sinc_half_angle = sinc_normx((r / T(2.0)).eval());
        Matrix3<T> K = Hat(r);
        Matrix3<T> K2 = K * K;
        Matrix3<T> R =
            sinc_angle * K + 0.5 * sinc_half_angle * sinc_half_angle * K2;
        R.diagonal().array() += T(1.0);
        return R;
    }
}

template <typename Derived, typename T>
Eigen::Quaternion<T> construct_quaternion(const Eigen::MatrixBase<Derived>& r)
{
    assert(r.size() == 3 && (r.rows() == 3 || r.cols() == 3));
    T angle = r.norm();
    if (angle == 0) {
        return Eigen::Quaternion<T>::Identity();
    }
    return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, r / angle));
}

template <typename T> Matrix3<T> rotate_to_z(Vector3<T> n)
{
    if (n.norm() == T(0)) {
        return Matrix3<T>::Identity();
    }
    return Eigen::Quaternion<T>::FromTwoVectors(n, Vector3<T>::UnitZ())
        .toRotationMatrix();
}

template <typename T> Matrix3<T> rotate_around_z(const T& theta)
{
    Matrix3<T> R;
    R.row(0) << cos(theta), -sin(theta), T(0);
    R.row(1) << sin(theta), cos(theta), T(0);
    R.row(2) << T(0), T(0), T(1);
    return R;
}

template <typename T>
void decompose_to_z_screwing(
    const Pose<T>& pose_t0,
    const Pose<T>& pose_t1,
    Matrix3<T>& R0,
    Matrix3<T>& P,
    T& omega)
{
    // Decompose the inbetween rotation as a rotation around the z-axis:
    //     R = Pᵀ R_z P
    // Where R = R₁R₀ᵀ, P is a rotation from n̂ to ẑ, and R_z is a rotation
    // of ω around the z-axis.
    R0 = pose_t0.construct_rotation_matrix();
    Matrix3<T> R1 = pose_t1.construct_rotation_matrix();
    Eigen::AngleAxis<T> r(R1 * R0.transpose());
    omega = r.angle();
    P = rotate_to_z(r.axis());
}

} // namespace ipc::rigid


#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/extract_indices.h>

#include <Eigen/Dense>

#include "minipcl.h"

// set ksearch and radius to < 0 to disable
void mpcl_compute_normals(pcl::PointCloud<pcl::PointXYZ> &cloud,
                          int ksearch,
                          double searchRadius,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud.makeShared());
    if (ksearch >= 0)
        ne.setKSearch (ksearch);
    if (searchRadius >= 0.0)
        ne.setRadiusSearch (searchRadius);
    ne.compute (out);
}

void mpcl_sacnormal_set_axis(pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> &sac,
                             double ax, double ay, double az)
{
    Eigen::Vector3f vect(ax,ay,az);
    sac.setAxis(vect);
}

void mpcl_extract(pcl::PointCloud<pcl::PointXYZ>::Ptr &incloud,
                  pcl::PointCloud<pcl::PointXYZ> *outcloud,
                  pcl::PointIndices *indices,
                  bool negative)
{
    pcl::PointIndices::Ptr indicesptr (indices);
    pcl::ExtractIndices<pcl::PointXYZ> ext;
    ext.setInputCloud(incloud);
    ext.setIndices(indicesptr);
    ext.setNegative(negative);
    ext.filter(*outcloud);
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief quantity.cpp

\details
Test quantity algebra.

Output:
@verbatim

//[quantity_output_double
L                                 = 2 m
L+L                               = 4 m
L-L                               = 0 m
L*L                               = 4 m^2
L/L                               = 1 dimensionless
L*meter                           = 2 m^2
kilograms*(L/seconds)*(L/seconds) = 4 m^2 kg s^-2
kilograms*(L/seconds)^2           = 4 m^2 kg s^-2
L^3                               = 8 m^3
L^(3/2)                           = 2.82843 m^(3/2)
2vL                               = 1.41421 m^(1/2)
(3/2)vL                           = 1.5874 m^(2/3)
//]

//[quantity_output_complex
L                                 = (3,4) m
L+L                               = (6,8) m
L-L                               = (0,0) m
L*L                               = (-7,24) m^2
L/L                               = (1,0) dimensionless
L*meter                           = (3,4) m^2
kilograms*(L/seconds)*(L/seconds) = (-7,24) m^2 kg s^-2
kilograms*(L/seconds)^2           = (-7,24) m^2 kg s^-2
L^3                               = (-117,44) m^3
L^(3/2)                           = (2,11) m^(3/2)
2vL                               = (2,1) m^(1/2)
(3/2)vL                           = (2.38285,1.69466) m^(2/3)
//]

@endverbatim
**/

#include <complex>
#include <iostream>

#include <boost/mpl/list.hpp>

#include <boost/typeof/std/complex.hpp>

#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>

#include "test_system.hpp"

int main(void)
{
    using namespace boost::units;
    using namespace boost::units::test;

    {
    //[quantity_snippet_1
    quantity<length> L = 2.0*meters;                     // quantity of length
    quantity<energy> E = kilograms*pow<2>(L/seconds);    // quantity of energy
    //]

    std::cout << "L                                 = " << L << std::endl
              << "L+L                               = " << L+L << std::endl
              << "L-L                               = " << L-L << std::endl
              << "L*L                               = " << L*L << std::endl
              << "L/L                               = " << L/L << std::endl
              << "L*meter                           = " << L*meter << std::endl
              << "kilograms*(L/seconds)*(L/seconds) = "
              << kilograms*(L/seconds)*(L/seconds) << std::endl
              << "kilograms*(L/seconds)^2           = "
              << kilograms*pow<2>(L/seconds) << std::endl
              << "L^3                               = "
              << pow<3>(L) << std::endl
              << "L^(3/2)                           = "
              << pow<static_rational<3,2> >(L) << std::endl
              << "2vL                               = "
              << root<2>(L) << std::endl
              << "(3/2)vL                           = "
              << root<static_rational<3,2> >(L) << std::endl
              << std::endl;
    }

    {
    //[quantity_snippet_2
    quantity<length,std::complex<double> > L(std::complex<double>(3.0,4.0)*meters);
    quantity<energy,std::complex<double> > E(kilograms*pow<2>(L/seconds));
    //]

    std::cout << "L                                 = " << L << std::endl
              << "L+L                               = " << L+L << std::endl
              << "L-L                               = " << L-L << std::endl
              << "L*L                               = " << L*L << std::endl
              << "L/L                               = " << L/L << std::endl
              << "L*meter                           = " << L*meter << std::endl
              << "kilograms*(L/seconds)*(L/seconds) = "
              << kilograms*(L/seconds)*(L/seconds) << std::endl
              << "kilograms*(L/seconds)^2           = "
              << kilograms*pow<2>(L/seconds) << std::endl
              << "L^3                               = "
              << pow<3>(L) << std::endl
              << "L^(3/2)                           = "
              << pow<static_rational<3,2> >(L) << std::endl
              << "2vL                               = "
              << root<2>(L) << std::endl
              << "(3/2)vL                           = "
              << root<static_rational<3,2> >(L) << std::endl
              << std::endl;
    }

    return 0;
}


#ifndef Polyhedron_HPP
#define Polyhedron_HPP
#include <iostream>
#include <fstream>
#include <vector>
#include "Vertex.hpp"
#include <string>
#include <boost/regex.hpp>

typedef unsigned int uint;
typedef std::vector<uint> face;

class Polyhedron
{
public:
	Polyhedron();
	~Polyhedron();

	void loadSTL(std::string file);

	void setName(std::string name);
	std::string getName();

	void addPoint(Vertex toAdd);
	void addPoint(double x, double y, double z);
	void addPoints(double toAdd[][3], uint nElem);

	void addFace(face toAdd);
	void addFace(uint* faceSet,uint nElem);
	//If all faces are the same dimension:
	void addFaces(uint* faceSet, uint nFaces, uint nElem);

	Vertex getPoint(uint index);
	face getFace(uint index);
	//This fonction suppose that all points
	//of the face are in one plan
	Vertex normalOfFace(uint index);
	bool isInside(Vertex source);
	bool isFaceConnected(uint indexA , uint indexB);
	//Only work is all faces are triangle
	bool isConvex();

	friend std::ostream& operator<<(std::ostream& out,Polyhedron& toPrint);

private:
	std::string name;
	std::vector<Vertex> vertices;
	std::vector<face> faces;
};

#endif


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sincosd.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/function/sind.hpp>
#include <boost/simd/function/cosd.hpp>

STF_CASE_TPL (" sincosd",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::sincosd;
  T a[] = {bs::Zero<T>(), bs::One<T>(), T(120), T(180),
           T(90), bs::Inf<T>(), bs::Minf<T>(), bs::Nan<T>()};
  size_t N =  sizeof(a)/sizeof(T);

  STF_EXPR_IS( (sincosd(T()))
             , (std::pair<T,T>)
             );

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<T,T> p = sincosd(a[i]);
      STF_IEEE_EQUAL(p.first,  bs::sind(a[i]));
      STF_IEEE_EQUAL(p.second, bs::cosd(a[i]));
      std::pair<T,T> q = bs::restricted_(sincosd)(a[i]);
      STF_IEEE_EQUAL(q.first,  bs::restricted_(bs::sind)(a[i]));
      STF_IEEE_EQUAL(q.second, bs::restricted_(bs::cosd)(a[i]));
    }
  }
}


#include <igl/copyleft/cgal/trim_with_solid.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/validate_arg.h>
#include <igl/C_STR.h>

#include <mex.h>
#include <Eigen/Dense>

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  mexErrMsgTxt(nrhs>=4,"Must have four inputs");
  Eigen::MatrixXd VA,VB;
  Eigen::MatrixXi FA,FB;
  const auto & parse_mesh = [](
      const mxArray *prhs[],
      MatrixXd & V,
      MatrixXi & F)
  {
    parse_rhs_double(prhs,V);
    parse_rhs_index(prhs+1,F);
    mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
    mexErrMsgTxt(F.cols()==3,"F must be #F by 3");
  };
  parse_mesh(prhs+0,VA,FA);
  parse_mesh(prhs+2,VB,FB);
  Eigen::MatrixXd V;
  Eigen::MatrixXi F;
  Eigen::VectorXi D,J;
  igl::copyleft::cgal::trim_with_solid(VA,FA,VB,FB,V,F,D,J);
  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 4:
    {
      prepare_lhs_index(J,plhs+3);
      // Fall through
    }
    case 3:
    {
      prepare_lhs_logical(D,plhs+2);
      // Fall through
    }
    case 2:
    {
      prepare_lhs_index(F,plhs+1);
      // Fall through
    }

    case 1:
    {
      prepare_lhs_double(V,plhs+0);
      // Fall through
    }
    case 0: break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}