#pragma once

#include "qnum.hpp"
#include "flex.hpp"
#include <Eigen/Core>
#include "autodiff/reverse.hpp"
/// Eigen3 supporting types and helpers

// Eigen specializations
namespace Eigen
{
  using namespace qnum;
  using namespace flex;
  using namespace autodiff;
  /// Traits specialization for qspace_number_t.
  /// See Eigen/src/Core/NumTraits.h for documentation.
  template<typename T, int E> struct NumTraits<qspace_number_t<T, E>>
    : GenericNumTraits<qspace_number_t<T, E>>
  {
    typedef qspace_number_t<T, E> Real;
    typedef qspace_number_t<T, E> NonInteger;
    typedef qspace_number_t<T, E> Nested;
    typedef qspace_number_t<T, E> Literal;

    enum {
      IsComplex = 0,
      IsInteger = 0,
      ReadCost = 1,
      AddCost = 1,
      MulCost = 1,
      IsSigned = 1,
      RequireInitialization = 1,
    };

    static inline Real epsilon() { return Real::from_literal(1, false); }
    static inline Real dummy_precision() { Real v; return v; }
    static inline Real highest() { return Real::from_literal(Real::T_max(), true); }
    static inline Real lowest() { return Real::from_literal(Real::T_min(), true); }
    static inline int digits10() { return std::numeric_limits<T>::digits10; }
  };

  namespace internal {
    /// Partial specialization for random implementation for qspace numbers.
    /// See MathFunctions.h L535
    template<typename T, int E> struct random_impl<qspace_number_t<T, E>>
      : random_default_impl
        <
        qspace_number_t<T, E>,
        NumTraits<qspace_number_t<T, E>>::IsComplex,
        NumTraits<qspace_number_t<T, E>>::IsInteger
        >
    {
      typedef qspace_number_t<T, E> _Q;
      static inline _Q run(const _Q& x, const _Q& y) {
        if (x > y) return x;

        int rn = std::rand() * RAND_MAX + std::rand();
        typename _Q::Tu xu = static_cast<typename _Q::Tu>(x.val);
        typename _Q::Tu yu = static_cast<typename _Q::Tu>(y.val);
        auto ru = static_cast<typename _Q::Tu>(rn) % yu - xu;
        return _Q::from_literal(static_cast<T>(ru), false);
      }
      static inline _Q run() {
        int rn = std::rand() * RAND_MAX + std::rand();
        return _Q::from_literal(static_cast<T>(rn) >> _Q::ext_bits(), false);
      }
    };

    template<typename T, int E> struct random_impl<Variable<qspace_number_t<T, E>>>
      : random_default_impl
        <
        Variable<qspace_number_t<T, E>>,
        NumTraits<Variable<qspace_number_t<T, E>>>::IsComplex,
        NumTraits<Variable<qspace_number_t<T, E>>>::IsInteger
        >
    {
      typedef qspace_number_t<T, E> _Q;
      static inline Variable<_Q> run(const Variable<_Q>& x, const Variable<_Q>& y) {
        return Variable<_Q>(random_impl<_Q>::run(x.expr->val, y.expr->val));
      }
      static inline Variable<_Q> run() {
        return Variable<_Q>(random_impl<_Q>::run());
      }
    };
  }

  /// Traits specialization for flexfloat.
  /// See Eigen/src/Core/NumTraits.h for documentation.
  template<uint8_t E, uint8_t F> struct NumTraits<flexfloat<E, F>>
    : NumTraits<double>
  {
    typedef flexfloat<E, F> Real;
    typedef flexfloat<E, F> NonInteger;
    typedef flexfloat<E, F> Nested;
    typedef flexfloat<E, F> Literal;

    enum {
      RequireInitialization = 1,
    };

    static inline Real epsilon() {
      Real v;
      flexfloat_t ff = (flexfloat_t)v;
      flexfloat_set_bits(&ff, 1);
      return (Real)ff;
    }
    static inline Real dummy_precision() {
      Real v;
      return v;
    }
    static inline Real highest() { return Real(FLT_MAX); }
    static inline Real lowest() { return Real(FLT_MIN);}
    static inline int digits10() { return 10; } // XXX wrong
  };

  namespace internal {
    /// Partial specialization for random implementation for flexfloat.
    /// See MathFunctions.h L535
    template<uint8_t E, uint8_t F> struct random_impl<flexfloat<E, F>>
      : random_default_impl
        <
        flexfloat<E, F>,
        NumTraits<flexfloat<E, F>>::IsComplex,
        NumTraits<flexfloat<E, F>>::IsInteger
        >
    {
      typedef flexfloat<E, F> _Q;
      static inline _Q run(const _Q& x, const _Q& y) {
        if (x > y) return x;
        double rn = std::rand();
        rn = rn / RAND_MAX * (y - x) + x;
        return (_Q)rn;
      }
      static inline _Q run() {
        double rn = std::rand();
        rn = (rn / RAND_MAX) * 2.0 - 1.0;
        return (_Q)rn;
      }
    };

    template<uint8_t E, uint8_t F> struct random_impl<Variable<flexfloat<E, F>>>
      : random_default_impl
        <
        Variable<flexfloat<E, F>>,
        NumTraits<Variable<flexfloat<E, F>>>::IsComplex,
        NumTraits<Variable<flexfloat<E, F>>>::IsInteger
        >
    {
      typedef flexfloat<E, F> _Q;
      static inline Variable<_Q> run(const Variable<_Q>& x, const Variable<_Q>& y) {
        return Variable<_Q>(random_impl<_Q>::run(x.expr->val, y.expr->val));
      }
      static inline Variable<_Q> run() {
        return Variable<_Q>(random_impl<_Q>::run());
      }
    };
  }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_DIVROUND_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_DIVROUND_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/divround.hpp>
#include <boost/simd/include/functions/scalar/round.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/zero.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)
                                   , (scalar_< arithmetic_<A0> >)
                                     (scalar_< arithmetic_<A0> >)
                                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
      {
        return (a1) ? static_cast<A0>(round(static_cast<double>(a0)/static_cast<double>(a1)))
          : ((a0 > 0) ? Valmax<A0>()
             : ((a0 < 0) ? Valmin<A0>()
                : Zero<A0>()
                )
             );
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)
                                   , (scalar_< unsigned_<A0> >)
                                     (scalar_< unsigned_<A0> >)
                                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
      {
        return (a1) ? static_cast<A0>(round(static_cast<double>(a0)/static_cast<double>(a1)))
          : ((a0 > 0) ? Valmax<A0>() : Zero<A0>()
             );
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)
                                   , (scalar_< floating_<A0> >)
                                     (scalar_< floating_<A0> >)
                                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
      {
        return boost::simd::round(a0/a1);
      }
  };
} } }

#endif


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

/*
 Copyright (C) 2015 Peter Caspers

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

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

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

#include <ql/termstructures/yield/clonedyieldtermstructure.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

ClonedYieldTermStructure::ClonedYieldTermStructure(
    const boost::shared_ptr<YieldTermStructure> source,
    const ReactionToTimeDecay reactionToTimeDecay,
    const Processing processing,
    const Calendar calendar)
    : YieldTermStructure(source->dayCounter()),
      reactionToTimeDecay_(reactionToTimeDecay),
      processing_(processing),
      originalEvalDate_(Settings::instance().evaluationDate()),
      originalReferenceDate_(Date(source->referenceDate())),
      originalMaxDate_(source->maxDate()) {

    calendar_ = calendar.empty() ? source->calendar() : calendar;
    QL_REQUIRE(!calendar_.empty() || reactionToTimeDecay_ == FixedReferenceDate,
               "a floating termstructure needs a calendar, none given and "
               "source termstructures' calendar is empty, too");

    referenceDate_ = originalReferenceDate_;
    maxDate_ = originalMaxDate_;
    offset_ = 0.0;
    valid_ = true;

    instFwdMax_ =
        source->forwardRate(maxDate_, maxDate_, Actual365Fixed(), Continuous)
            .rate();

    if (reactionToTimeDecay != FixedReferenceDate) {
        QL_REQUIRE(originalReferenceDate_ >= originalEvalDate_,
                   "to construct a moving term structure the source term "
                   "structure must have a reference date ("
                       << originalReferenceDate_
                       << ") after the evaluation date ("
                       << originalEvalDate_
                       << ")");
            try {
                impliedSettlementDays_ = source->settlementDays();
            } catch(...) {
                // if the source ts has no settlement days we imply
                // them from the difference of the original reference
                // date and the original evaluation date
                impliedSettlementDays_ = this->calendar().businessDaysBetween(
                    originalEvalDate_, originalReferenceDate_);
            }
    }

    discounts_.resize(originalMaxDate_.serialNumber() -
                      originalReferenceDate_.serialNumber()+1);
    times_.resize(discounts_.size());

    for (BigInteger i = 0; i <= originalMaxDate_.serialNumber() -
                                    originalReferenceDate_.serialNumber();
         ++i) {
        Date d = Date(originalReferenceDate_.serialNumber()
                      +i);
        discounts_[i] = source->discount(d);
        times_[i] = timeFromReference(d);
        if(processing == PositiveYieldsAndForwards) {
            discounts_[i] = std::min(1.0, discounts_[i]);
        }
        if (processing == PositiveForwards ||
            processing == PositiveYieldsAndForwards) {
            if (i > 0)
                discounts_[i] = std::min(discounts_[i - 1], discounts_[i]);
        }
    }

    interpolation_ = boost::make_shared<LogLinearInterpolation>(
        times_.begin(), times_.end(), discounts_.begin());
    interpolation_->update();

    if (reactionToTimeDecay_ != FixedReferenceDate) {
        registerWith(Settings::instance().evaluationDate());
    }
}

DiscountFactor ClonedYieldTermStructure::discountImpl(Time t) const {
    QL_REQUIRE(valid_, "termstructure not valid, evaluation date ("
                           << Settings::instance().evaluationDate()
                           << ") is before the evaluation date when the "
                              "termstructure was frozen ("
                           << originalEvalDate_);
    Time tMax = maxTime();
    Time tEff = t + offset_;
    if (tEff < tMax) {
        // also ok for offset_ = 0
        return interpolation_->operator()(tEff) /
               interpolation_->operator()(offset_);
    }

    // flat fwd extrapolation
    DiscountFactor dMax = discounts_.back();
    return dMax * std::exp(-instFwdMax_ * (tEff - tMax));
}

void ClonedYieldTermStructure::update() {
    YieldTermStructure::update();
    if (reactionToTimeDecay_ != FixedReferenceDate) {
        Date today = Settings::instance().evaluationDate();
        if (today < originalEvalDate_) {
            valid_ = false;
        } else {
            valid_ = true;
            referenceDate_ = calendar().advance(today, impliedSettlementDays_ * Days);
            if (reactionToTimeDecay_ == ForwardForward) {
                offset_ = dayCounter().yearFraction(originalReferenceDate_,
                                                    referenceDate_);
            }
            if (reactionToTimeDecay_ == ConstantZeroYields) {
                BigNatural dayOffset = referenceDate_ - originalReferenceDate_;
                maxDate_ = Date(std::min<BigNatural>(
                    originalMaxDate_.serialNumber() + dayOffset,
                    Date::maxDate().serialNumber()));
            }
        }
    }
}

} // namespace QuantLib


#pragma once
#include <armadillo>
#include "project/LinearRegression.hpp"

class AutoregressiveModel: public LinearRegression
{
    private:
        int reg_lag;
        arma::vec ts;
    public:
        AutoregressiveModel(arma::vec &x, arma::vec &y, int lag);
        arma::mat laggedMatrix(arma::vec &x, int lag);
        float pointPrediction(arma::mat &x);
        arma::vec forecast(int horizon);
};


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/constants.h>
#include <scitbx/math/mean_and_variance.h>
#include <scitbx/vec3.h>
#include <cctbx/miller.h>

#include <vector>
#include <map>

using namespace boost::python;

namespace xfel {

struct mark2_iteration {
  typedef scitbx::af::shared<double> farray;
  typedef scitbx::af::shared<int> iarray;
  farray values;
  farray tox;
  farray toy;
  farray spotcx;
  farray spotcy;
  farray spotfx;
  farray spotfy;
  iarray master_tiles;
  double functional;
  farray gradients_,curvatures_;

  farray model_calcx, model_calcy;
  double calc_minus_To_x, calc_minus_To_y;
  double rotated_o_x, rotated_o_y;
  double partial_partial_theta_x, partial_partial_theta_y;
  double partial_sq_theta_x, partial_sq_theta_y;

  farray sine,cosine;

  mark2_iteration(){}
  mark2_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles):
    values(values),tox(tox),toy(toy),spotcx(spotcx),spotcy(spotcy),
    master_tiles(master_tiles),
    model_calcx(spotcx.size(),scitbx::af::init_functor_null<double>()),
    model_calcy(spotcx.size(),scitbx::af::init_functor_null<double>()),
    functional(0.),
    gradients_(3*64),
    curvatures_(3*64)
  {
    SCITBX_ASSERT(tox.size()==64);
    SCITBX_ASSERT(toy.size()==64);
    SCITBX_ASSERT(values.size()==3*64);
    for (int tidx=0; tidx < 64; ++tidx){
      cosine.push_back(std::cos(values[128+tidx]*scitbx::constants::pi_180));
      sine.push_back(std::sin(values[128+tidx]*scitbx::constants::pi_180));
    }

    for (int ridx=0; ridx < spotcx.size(); ++ridx){
      int itile = master_tiles[ridx];
      calc_minus_To_x = spotcx[ridx] - tox[itile];
      calc_minus_To_y = spotcy[ridx] - toy[itile];

      rotated_o_x = calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];
      rotated_o_y = calc_minus_To_x * sine[itile] +   calc_minus_To_y * cosine[itile];

      model_calcx[ridx] = rotated_o_x + (tox[itile] + values[2*itile]);
      model_calcy[ridx] = rotated_o_y + (toy[itile] + values[2*itile+1]);

      partial_partial_theta_x = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];
      partial_partial_theta_y =  calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];

      partial_sq_theta_x = -calc_minus_To_x * cosine[itile] + calc_minus_To_y * sine[itile];
      partial_sq_theta_y = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];

      double delx = model_calcx[ridx] - spotfx[ridx];
      double dely = model_calcy[ridx] - spotfy[ridx];
      double delrsq(delx*delx + dely*dely);
      functional += delrsq; // sum of square differences

      gradients_[2*itile]  += 2. *  delx;
      gradients_[2*itile+1]+= 2. *  dely;

      gradients_[128+itile] += scitbx::constants::pi_180 * 2.* (
        delx * partial_partial_theta_x +
        dely * partial_partial_theta_y
      );

      curvatures_[2*itile] += 2.;
      curvatures_[2*itile+1] += 2.;

      curvatures_[128+itile] += scitbx::constants::pi_180 * scitbx::constants::pi_180 * 2. * (
        ( partial_partial_theta_x*partial_partial_theta_x +
          partial_partial_theta_y*partial_partial_theta_y ) +
        ( delx*partial_sq_theta_x + dely*partial_sq_theta_y )
      );
    }
  }
  mark2_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles,iarray frames,int const& nframes,
                  bool const& inheritable):
    values(values),tox(tox),toy(toy),spotcx(spotcx),spotcy(spotcy),
    master_tiles(master_tiles),
    model_calcx(spotcx.size(),scitbx::af::init_functor_null<double>()),
    model_calcy(spotcx.size(),scitbx::af::init_functor_null<double>()),
    functional(0.){}

  mark2_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles,iarray frames,int const& nframes):
    values(values),tox(tox),toy(toy),spotcx(spotcx),spotcy(spotcy),
    master_tiles(master_tiles),
    model_calcx(spotcx.size(),scitbx::af::init_functor_null<double>()),
    model_calcy(spotcx.size(),scitbx::af::init_functor_null<double>()),
    functional(0.),
    gradients_(3*64+2*nframes),
    curvatures_(3*64+2*nframes)
  {
    SCITBX_ASSERT(tox.size()==64);
    SCITBX_ASSERT(toy.size()==64);
    SCITBX_ASSERT(values.size()==3*64+2*nframes);
    for (int tidx=0; tidx < 64; ++tidx){
      cosine.push_back(std::cos(values[128+tidx]*scitbx::constants::pi_180));
      sine.push_back(std::sin(values[128+tidx]*scitbx::constants::pi_180));
    }

    for (int ridx=0; ridx < spotcx.size(); ++ridx){
      int itile = master_tiles[ridx];
      int frame_param_no = frames[ridx];
      calc_minus_To_x = spotcx[ridx] - tox[itile];
      calc_minus_To_y = spotcy[ridx] - toy[itile];

      rotated_o_x = calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];
      rotated_o_y = calc_minus_To_x * sine[itile] +   calc_minus_To_y * cosine[itile];

      model_calcx[ridx] = rotated_o_x + (tox[itile] + values[2*itile]);
      model_calcy[ridx] = rotated_o_y + (toy[itile] + values[2*itile+1]);

      partial_partial_theta_x = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];
      partial_partial_theta_y =  calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];

      partial_sq_theta_x = -calc_minus_To_x * cosine[itile] + calc_minus_To_y * sine[itile];
      partial_sq_theta_y = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];

      double delx = model_calcx[ridx] - spotfx[ridx];
      double dely = model_calcy[ridx] - spotfy[ridx];
      double delrsq(delx*delx + dely*dely);
      functional += delrsq; // sum of square differences

      gradients_[2*itile]  += 2. *  delx;
      gradients_[2*itile+1]+= 2. *  dely;
      if (frame_param_no < nframes){
        gradients_[192+2*frame_param_no]  += 2. *  delx;
        gradients_[193+2*frame_param_no]  += 2. *  dely;
      }

      gradients_[128+itile] += scitbx::constants::pi_180 * 2.* (
        delx * partial_partial_theta_x +
        dely * partial_partial_theta_y
      );

      curvatures_[2*itile] += 2.;
      curvatures_[2*itile+1] += 2.;
      if (frame_param_no < nframes){
        curvatures_[192+2*frame_param_no] += 2.;
        curvatures_[193+2*frame_param_no] += 2.;
      }

      curvatures_[128+itile] += scitbx::constants::pi_180 * scitbx::constants::pi_180 * 2. * (
        ( partial_partial_theta_x*partial_partial_theta_x +
          partial_partial_theta_y*partial_partial_theta_y ) +
        ( delx*partial_sq_theta_x + dely*partial_sq_theta_y )
      );
    }
  }

  double f(){ return functional; }
  farray gradients(){ return gradients_; }
  farray curvatures(){ return curvatures_; }
};

struct mark3_collect_data{
  //adapt all-frame data to individual-frame parameter refinement
  typedef scitbx::af::shared<double> farray;
  typedef scitbx::af::shared<int> iarray;
  typedef scitbx::af::shared<cctbx::miller::index<> > marray;
  typedef scitbx::af::shared<bool> barray;
  marray HKL;
  std::map<int,int> frame_first_index, frame_match_count;
  farray result_model_cx,result_model_cy;
  barray result_flags;
  scitbx::af::shared<scitbx::vec3<double> > result_part_distance;

  mark3_collect_data(){}
  mark3_collect_data(iarray frame_id, marray indices):
    HKL(indices),
    result_model_cx(frame_id.size(),scitbx::af::init_functor_null<double>()),
    result_model_cy(frame_id.size(),scitbx::af::init_functor_null<double>()),
    result_flags(frame_id.size(),scitbx::af::init_functor_null<bool>()),
    result_part_distance(frame_id.size(),scitbx::af::init_functor_null<scitbx::vec3<double> >())
  {
    SCITBX_ASSERT(frame_id.size()==indices.size());

    for (int idx=0; idx < frame_id.size(); ++idx){
      int iframe = frame_id[idx];
      if (frame_first_index.find(iframe)==frame_first_index.end()){
        frame_first_index[iframe]=idx;
        frame_match_count[iframe]=1;
      } else {
        SCITBX_ASSERT(
          frame_first_index[iframe]+frame_match_count[iframe] == idx);// each frame all contiguous
        frame_match_count[iframe]+=1;
      }
    }
  }

  int
  get_first_index(int const& frame_id) const {
    return frame_first_index.find(frame_id)->second;
  }

  marray
  frame_indices(int const& frame_id)const{
    marray result;
    int last = frame_first_index.find(frame_id)->second + frame_match_count.find(frame_id)->second;
    for (int idx=frame_first_index.find(frame_id)->second; idx < last; ++idx){
      result.push_back(HKL[idx]);
    }
    return result;
  }

  barray
  selection(int const& frame_id)const{
    barray result(result_model_cx.size());
    int last = frame_first_index.find(frame_id)->second + frame_match_count.find(frame_id)->second;
    for (int idx=frame_first_index.find(frame_id)->second; idx < last; ++idx){
      result[idx] = true;
    }
    return result;
  }

  void collect(scitbx::af::shared<scitbx::vec3<double> > hi_E_limit,
               scitbx::af::shared<scitbx::vec3<double> > lo_E_limit,
               barray observed_flag,
               int const& frame_id){
    int first = frame_first_index.find(frame_id)->second;
    for (int im = 0; im < hi_E_limit.size(); ++im){
      result_model_cx[first + im] = (hi_E_limit[im][1] + lo_E_limit[im][1])/2.;
      result_model_cy[first + im] = (hi_E_limit[im][0] + lo_E_limit[im][0])/2.;
      result_flags[first + im] = observed_flag[im];
      SCITBX_ASSERT (observed_flag[im]); // no current support for masked-out spots
    }
  }
  void collect_mean_position(scitbx::af::shared<scitbx::vec3<double> > mean_position,
               barray observed_flag,
               int const& frame_id){
    int first = frame_first_index.find(frame_id)->second;
    for (int im = 0; im < mean_position.size(); ++im){
      result_model_cx[first + im] = mean_position[im][1];
      result_model_cy[first + im] = mean_position[im][0];
      result_flags[first + im] = observed_flag[im];
      SCITBX_ASSERT (observed_flag[im]); // no current support for masked-out spots
    }
  }
  void collect_distance(scitbx::af::shared<scitbx::vec3<double> > part_distance,
               int const& frame_id){
    int first = frame_first_index.find(frame_id)->second;
    for (int im = 0; im < part_distance.size(); ++im){
      result_part_distance[first + im] = part_distance[im];
    }
  }
};

struct mark5_iteration: public mark2_iteration {
  typedef scitbx::af::shared<scitbx::vec3<double> > vec3array;
  typedef scitbx::vec3<double>                      vec3;

  mark5_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles,iarray frames,int const& nframes,
                  vec3array partial_r_partial_distance):
    mark2_iteration(values, tox, toy, spotcx, spotcy, spotfx, spotfy,
                    master_tiles, frames, nframes, true)
  {
    gradients_ = farray(3*64+3*nframes);
    curvatures_ = farray(3*64+3*nframes);
    SCITBX_ASSERT(tox.size()==64);
    SCITBX_ASSERT(toy.size()==64);
    SCITBX_ASSERT(values.size()==3*64+3*nframes);
    for (int tidx=0; tidx < 64; ++tidx){
      cosine.push_back(std::cos(values[128+tidx]*scitbx::constants::pi_180));
      sine.push_back(std::sin(values[128+tidx]*scitbx::constants::pi_180));
    }

    for (int ridx=0; ridx < spotcx.size(); ++ridx){
      int itile = master_tiles[ridx];
      int frame_param_no = frames[ridx];

      calc_minus_To_x = spotcx[ridx] - tox[itile];
      calc_minus_To_y = spotcy[ridx] - toy[itile];

      rotated_o_x = calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];
      rotated_o_y = calc_minus_To_x * sine[itile] +   calc_minus_To_y * cosine[itile];

      model_calcx[ridx] = rotated_o_x + (tox[itile] + values[2*itile]);
      model_calcy[ridx] = rotated_o_y + (toy[itile] + values[2*itile+1]);

      partial_partial_theta_x = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];
      partial_partial_theta_y =  calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];

      partial_sq_theta_x = -calc_minus_To_x * cosine[itile] + calc_minus_To_y * sine[itile];
      partial_sq_theta_y = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];

      double delx = model_calcx[ridx] - spotfx[ridx];
      double dely = model_calcy[ridx] - spotfy[ridx];
      double delrsq(delx*delx + dely*dely);
      functional += delrsq; // sum of square differences

      vec3 part_r_part_d = partial_r_partial_distance[ridx];
      double rotated_part_r_c_x = part_r_part_d[0] * cosine[itile] - part_r_part_d[1] * sine[itile];
      double rotated_part_r_c_y = part_r_part_d[0] * sine[itile] +   part_r_part_d[1] * cosine[itile];

      //if (frame_param_no==10){
      //        printf("%5d %8.4f %8.4f", itile, part_r_part_d[0], part_r_part_d[1]);
      //        printf("%5d %8.4f %8.4f\n", itile, rotated_part_r_c_x, rotated_part_r_c_y);
      //}

      gradients_[2*itile]  += 2. *  delx;
      gradients_[2*itile+1]+= 2. *  dely;
      if (frame_param_no < nframes){
        gradients_[192+2*frame_param_no]  += 2. *  delx;
        gradients_[193+2*frame_param_no]  += 2. *  dely;
        gradients_[192 + 2*nframes + frame_param_no] += 2. * (
          delx * rotated_part_r_c_y + dely * rotated_part_r_c_x
        );//  SOMETHING IS ROTTEN IN DENMARK
      }

      gradients_[128+itile] += scitbx::constants::pi_180 * 2.* (
        delx * partial_partial_theta_x +
        dely * partial_partial_theta_y
      );

      curvatures_[2*itile] += 2.;
      curvatures_[2*itile+1] += 2.;
      if (frame_param_no < nframes){
        curvatures_[192+2*frame_param_no] += 2.;
        curvatures_[193+2*frame_param_no] += 2.;
        curvatures_[192 + 2*nframes + frame_param_no] +=2 * (
          rotated_part_r_c_x * rotated_part_r_c_x + rotated_part_r_c_y * rotated_part_r_c_y
        );
      }

      curvatures_[128+itile] += scitbx::constants::pi_180 * scitbx::constants::pi_180 * 2. * (
        ( partial_partial_theta_x*partial_partial_theta_x +
          partial_partial_theta_y*partial_partial_theta_y ) +
        ( delx*partial_sq_theta_x + dely*partial_sq_theta_y )
      );
    }

  }
};

namespace boost_python { namespace {

  void
  metrology_init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;

    class_<mark2_iteration>("mark2_iteration",no_init)
      .def(init< >())
      .def(init<mark2_iteration::farray, mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::iarray >(
        (arg_("values"),arg("tox"),arg_("toy"),arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"))))
      .def(init<mark2_iteration::farray, mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::iarray, mark2_iteration::iarray, int const& >(
        (arg_("values"),arg("tox"),arg_("toy"),arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"), arg_("frames"), arg_("nframes"))))
      .def("f",&mark2_iteration::f)
      .def("gradients",&mark2_iteration::gradients)
      .def("curvatures",&mark2_iteration::curvatures)
      .add_property("model_calcx", make_getter(&mark2_iteration::model_calcx, rbv()))
      .add_property("model_calcy", make_getter(&mark2_iteration::model_calcy, rbv()))
    ;
    class_<mark3_collect_data>("mark3_collect_data",no_init)
      .def(init<mark3_collect_data::iarray,mark3_collect_data::marray>())
      .def("get_first_index",&mark3_collect_data::get_first_index)
      .def("frame_indices",&mark3_collect_data::frame_indices)
      .def("selection",&mark3_collect_data::selection)
      .def("collect",&mark3_collect_data::collect)
      .def("collect_mean_position",&mark3_collect_data::collect_mean_position)
      .add_property("cx", make_getter(&mark3_collect_data::result_model_cx, rbv()))
      .add_property("cy", make_getter(&mark3_collect_data::result_model_cy, rbv()))
      .add_property("flags", make_getter(&mark3_collect_data::result_flags, rbv()))
      .def("collect_distance",&mark3_collect_data::collect_distance)
      .add_property("part_distance", make_getter(&mark3_collect_data::result_part_distance, rbv()))
    ;
    class_<mark5_iteration, bases<mark2_iteration> >("mark5_iteration",no_init)
      .def(init<mark5_iteration::farray, mark5_iteration::farray, mark5_iteration::farray,
                mark5_iteration::farray, mark5_iteration::farray,
                mark5_iteration::farray, mark5_iteration::farray,
                mark5_iteration::iarray, mark5_iteration::iarray, int const&,
                mark5_iteration::vec3array >(
        (arg_("values"),arg("tox"),arg_("toy"),arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"), arg_("frames"), arg_("nframes"), arg_("part_distance"))))
    ;
}
}}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(xfel_metrology_ext)
{
  xfel::boost_python::metrology_init_module();

}


/*****************************************************************************
*
* Functions for calculating the gap
*
* Copyright (C) 2018 by Hidemaro Suwa
* e-mail:suwamaro@phys.s.u-tokyo.ac.jp
*
*****************************************************************************/

#include "calc_gap.h"
#include "rpa_util.h"
#include <armadillo>

cx_double calc_intensity_cubic(int L, hoppings const& ts, double mu, double U, double delta, double qx, double qy, double qz, cx_double omega, bool zz){
  double k1 = 2. * M_PI / (double)L;

  cx_double A = 0, B = 0, C = 0, D = 0;
  /* Summing up at all the wavevectors */
  for(int x=-L/2; x < L/2; x++){
    double kx = k1 * x;
    for(int y=-L/2; y < L/2; y++){
      double ky = k1 * y;
      for(int z=-L/2; z < L/2; z++){
	double kz = k1 * z;

	add_to_sus_mat2( ts, mu, A, B, C, D, qx, qy, qz, kx, ky, kz, delta, omega, zz );
	// double e_free = energy_free_electron( t, mu, kx, ky, kz );
	// double e_eps = 1e-12;
	// if ( e_free < e_eps ) {
	//   /* Summing up over all k inside the Brillouin zone. */
	//   double diff_x = wave_vector_in_BZ( kx - qx );
	//   double diff_y = wave_vector_in_BZ( ky - qy );
	//   double diff_z = wave_vector_in_BZ( kz - qz );
	//   double e_free2 = energy_free_electron( t, mu, diff_x, diff_y, diff_z );
	//   add_to_sus_mat( A, B, D, e_free, e_free2, delta, omega );
	// }
      }
    }
  }

  int n_sites = L * L * L;
  A *= 2. / (double)n_sites;
  B *= 2. / (double)n_sites;
  C *= 2. / (double)n_sites;
  D *= 2. / (double)n_sites;

  /* RPA */
  arma::cx_mat chi0_mat(2,2);
  chi0_mat(0,0) = A;   // (A, A) correlation
  chi0_mat(0,1) = B;   // (A, B)
  chi0_mat(1,0) = C;   // (B, A)
  chi0_mat(1,1) = D;   // (B, B)

  /* Transverse = < \sigma^- \sigma^+ >; Longitudinal (zz) = < \sigma^z \sigma^z > */
  /* Note that 2 < \sigma^- \sigma^+ > = < \sigma^z \sigma^z > (U -> 0 for the SU(2) case) */
  double factor_channel = 1.0;
  if ( zz ) { factor_channel = 0.5; }

  arma::cx_mat denom = arma::eye<arma::cx_mat>(2,2) - factor_channel * U * chi0_mat;
  arma::cx_mat chi_mat = chi0_mat * arma::inv(denom);

  // sigma-to-spin factor
  double factor_operator = 0.5;
  cx_double chi = factor_operator * factor_operator * ( chi_mat(0,0) - chi_mat(1,0) - chi_mat(0,1) + chi_mat(1,1) );

  return chi;

  // A *= 2. / (double)n_sites;
  // B *= 2. / (double)n_sites;
  // D *= 2. / (double)n_sites;

  // /* RPA */
  // arma::cx_mat chi0_mat(2,2);
  // chi0_mat(0,0) = A;   // (A, A) correlation
  // chi0_mat(0,1) = B;   // (A, B)
  // chi0_mat(1,0) = B;   // (B, A)
  // chi0_mat(1,1) = D;   // (B, B)
  // arma::cx_mat denom = arma::eye<arma::cx_mat>(2,2) - U * chi0_mat;
  // arma::cx_mat chi_mat = chi0_mat * arma::inv(denom);

  // // Double counting from A and B
  // double factor_sublattice = 0.5;
  // cx_double chi = factor_sublattice * factor_sublattice * ( chi_mat(0,0) - chi_mat(1,0) - chi_mat(0,1) + chi_mat(1,1) );

  // // // for check
  // // std::cout << qx << "  " << qy << "  " << omega << "  " << A << "  " << B << "  " << D << "  " << chi_mat(0,0) << "  " << chi_mat(0,1) << "  " << chi_mat(1,0) << "  " << chi_mat(1,1) << "  " << chi << std::endl;

  // return chi;
}


#include "powerlawCommon.h"
#include <boost/program_options.hpp>

/**
 * @author: W.M. Otte (wim@invivonmr.uu.nl); Image Sciences Institute, UMC Utrecht, NL.
 * @date: 19-11-2009
 *
 * Estimate powerlaw scaling parameter from input distribution.
 *
 * ***************************************************************************
 * Method: "Power-law distributions in empirical data", Clauset et al, 2009
 * http://www.santafe.edu/~aaronc/powerlaws/
 * ***************************************************************************
 */
class PowerLawFit
{

public:

	typedef double ValueType;
	typedef std::vector< ValueType > VectorType;

	/**
	 * Power law fit.
	 */
	void run( const std::string& inputFileName, bool nosmall, bool finite,
					double startXmin, double incrementXmin, double endXmin,
						bool bootstrap, unsigned int bootstrapIterations, bool verbose )
	{
		// [ 1 ] read input from text file ...
		VectorType values = getInput( inputFileName );

		// [ 2 ] bootstrap or single fit ...
		VectorType results;

		if ( bootstrap )
		{
			graph::Powerlaw< ValueType >::BootstrapFit( values, results, nosmall, finite, startXmin, incrementXmin, endXmin, bootstrapIterations, verbose );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) <<  std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
				std::cout << "Alpha_sd," << results.at( 3 ) <<  std::endl;
				std::cout << "Xmin_sd," << results.at( 4 ) << std::endl;
				std::cout << "Log-likelihood_sd," << results.at( 5 ) << std::endl;

			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"bootstrap estimation failed! -> check input ..." << std::endl;
			}

		}
		else
		{
			graph::Powerlaw< ValueType >::SingleFit( values, results, nosmall, finite,
					startXmin, incrementXmin, endXmin );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) << std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"single estimation failed! -> check input ..." << std::endl;
			}
		}
	}

protected:

	/**
	 * Return input from given text file as vector.
	 */
	VectorType getInput( const std::string& input )
	{
		std::ifstream inFile;

		inFile.open( input.c_str() );
		if ( !inFile )
		{
			std::cout << "*** ERROR ***: Unable to open: " << input << "." << std::endl;
			exit( EXIT_FAILURE );
		}

		double x;
		VectorType output;

		while ( inFile >> x )
		{
			output.push_back( x );
		}
		inFile.close();

		/**
		 * Negative values will be converted to complex numbers in matlab,
		 * but not with the stl ...
		 *
		 * No support is given (yet) for complex number mle.
		 */
		if ( *( std::min_element( output.begin(), output.end() ) ) < 0 )
		{
			std::cerr << "*** ERROR ***: Negative input not supported!" << std::endl;
			exit (EXIT_FAILURE );
		}

		return output;
	}
};

// ************************************************************************************

/**
 * Throw error if required option is not specified.
 */
void required_option( const boost::program_options::variables_map& vm,
		const std::string& required_option )
{
	if ( vm.count( required_option ) == 0 )
		throw std::logic_error( "Option: '" + required_option + "' is required!" );
}

/**
 * Fit powerlaw to list of numbers.
 */
int main(int argc, char* argv[])
{
	namespace po = boost::program_options;

	// application description ...
	std::string description = "Fits a power-law distributional model to data.\n";

	// options ...
	std::string input;

	bool nosmall;
	bool finite;
	bool bootstrap;
	bool verbose;

	double startXmin;
	double incrementXmin;
	double endXmin;

	unsigned int bootstrapIterations;

	try {

        po::options_description desc("Available options");

        desc.add_options()

            ( "input,i", po::value< std::string >( &input )
            		, "string: input file with distribution values in column format." )

            ( "finite,f", po::value< bool >( &finite )
            		->default_value( false )
            		->zero_tokens()
            		, "bool: use an experimental finite-size correction." )

            ( "verbose,v", po::value< bool >( &verbose )
					->default_value( false )
            		->zero_tokens()
            		, "bool: print bootstrap status." )

            ( "nosmall,s", po::value< bool >( &nosmall )
            		->default_value( false )
            		->zero_tokens()
            		, "bool: truncate the search over xmin values before the finite-size bias becomes significant." )

            ( "bootstrap,b", po::value< bool >( &bootstrap )
            		->default_value( false )
            		->zero_tokens()
            		, "bool: run non-parametric bootstrap instead of single estimation." )

            ( "start-xmin,x", po::value< double >( &startXmin )
            		->default_value( 1.5 )
            		, "float: start value for discrete xmin estimation." )

            ( "increment-xmin,y", po::value< double >( &incrementXmin )
            		->default_value( 0.01 )
            		, "float: increment value for discrete xmin estimation." )

            ( "end-xmin,z", po::value< double >( &endXmin )
            		->default_value( 3.5 )
            		, "float: end value for discrete xmin estimation." )

            ( "bootstrap-iterations,n", po::value< unsigned int >( &bootstrapIterations )
            		->default_value( 1000 )
            		, "uint: bootstrap iterations." )

            ( "help,h", "bool: produce help message." )
        ;

        po::variables_map vm;
        po::store( po::parse_command_line( argc, argv, desc ), vm );
        po::notify( vm );

        // help message ...
        if ( vm.count( "help" ) )
        {
            std::cout << argv[0] << ": " << description << std::endl;
        	std::cout << desc << "\n";

        	return EXIT_SUCCESS;
        }

        // required options ...
        required_option( vm, "input" );

        // run application ...
        PowerLawFit powerlawFit;

    	powerlawFit.run( input, nosmall, finite,
							startXmin, incrementXmin, endXmin,
											bootstrap, bootstrapIterations, verbose );

    }
    catch( std::exception& e )
    {
        std::cerr << "*** ERROR ***: " << e.what() << "\n";
        std::cerr << "Use \"" << argv[0]
        		  << " --help\" for information about application usage."
        		  << std::endl;

        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}


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

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

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

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

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

/** \example amg.cpp
*
*   This tutorial shows the use of algebraic multigrid (AMG) preconditioners.
*   \warning AMG is currently only experimentally available with the OpenCL backend and depends on Boost.uBLAS
*
*   We start with some rather general includes and preprocessor variables:
**/

#ifndef NDEBUG     //without NDEBUG the performance of sparse ublas matrices is poor.
 #define BOOST_UBLAS_NDEBUG
#endif

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

#define VIENNACL_WITH_UBLAS 1

#include "viennacl/vector.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "viennacl/linalg/norm_2.hpp"

/**
* Import the AMG functionality:
**/
#include "viennacl/linalg/amg.hpp"

/**
* Some more includes:
**/
#include <iostream>
#include <vector>
#include <ctime>
#include "vector-io.hpp"

/**
Part 1: Worker routines

*
*
Run the Solver

*   Runs the provided solver specified in the `solver` object with the provided preconditioner `precond`
**/
template<typename MatrixType, typename VectorType, typename SolverTag, typename PrecondTag>
void run_solver(MatrixType const & matrix, VectorType const & rhs, VectorType const & ref_result, SolverTag const & solver, PrecondTag const & precond)
{
  VectorType result(rhs);
  VectorType residual(rhs);

  result = viennacl::linalg::solve(matrix, rhs, solver, precond);
  residual -= viennacl::linalg::prod(matrix, result);
  std::cout << "  > Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(rhs) << std::endl;
  std::cout << "  > Iterations: " << solver.iters() << std::endl;
  result -= ref_result;
  std::cout << "  > Relative deviation from result: " << viennacl::linalg::norm_2(result) / viennacl::linalg::norm_2(ref_result) << std::endl;
}

/**
Compare AMG preconditioner for uBLAS and ViennaCL types

*
*  The AMG implementations in ViennaCL can be used with uBLAS types as well as ViennaCL types.
*  This function compares the two in terms of execution time.
**/
template<typename ScalarType>
void run_amg(viennacl::linalg::cg_tag & cg_solver,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_vec*/,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_result*/,
             boost::numeric::ublas::compressed_matrix<ScalarType> & ublas_matrix,
             viennacl::vector<ScalarType> & vcl_vec,
             viennacl::vector<ScalarType> & vcl_result,
             viennacl::compressed_matrix<ScalarType> & vcl_compressed_matrix,
             std::string info,
             viennacl::linalg::amg_tag & amg_tag)
{

  viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > ublas_amg = viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > (ublas_matrix, amg_tag);
  boost::numeric::ublas::vector<ScalarType> avgstencil;
  unsigned int coarselevels = amg_tag.get_coarselevels();

  std::cout << "-- CG with AMG preconditioner, " << info << " --" << std::endl;

  std::cout << " * Setup phase (ublas types)..." << std::endl;

  // Coarse level measure might have been changed during setup. Reload!
  ublas_amg.tag().set_coarselevels(coarselevels);
  ublas_amg.setup();

  std::cout << " * Operator complexity: " << ublas_amg.calc_complexity(avgstencil) << std::endl;

  amg_tag.set_coarselevels(coarselevels);
  viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > vcl_amg = viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > (vcl_compressed_matrix, amg_tag);
  std::cout << " * Setup phase (ViennaCL types)..." << std::endl;
  vcl_amg.tag().set_coarselevels(coarselevels);
  vcl_amg.setup();

  std::cout << " * CG solver (ublas types)..." << std::endl;
  //run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, ublas_amg);

  std::cout << " * CG solver (ViennaCL types)..." << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, vcl_amg);

}

/**
*
Part 2: Run Solvers with AMG Preconditioners

*
*  In this
**/
int main()
{
  /**
  * Print some device info at the beginning. If there is more than one OpenCL device available, use the second device.
  **/
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "               Device Info" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  // Optional: Customize OpenCL backend
  viennacl::ocl::platform pf = viennacl::ocl::get_platforms()[0];
  std::vector<viennacl::ocl::device> const & devices = pf.devices();

  // Optional: Set first device to first context:
  viennacl::ocl::setup_context(0, devices[0]);

  // Optional: Set second device for second context (use the same device for the second context if only one device available):
  if (devices.size() > 1)
    viennacl::ocl::setup_context(1, devices[1]);
  else
    viennacl::ocl::setup_context(1, devices[0]);

  std::cout << viennacl::ocl::current_device().info() << std::endl;
  viennacl::context ctx(viennacl::ocl::get_context(1));
#else
  viennacl::context ctx;
#endif

  typedef float    ScalarType;  // feel free to change this to double if supported by your device

  /**
  * Set up the matrices and vectors for the iterative solvers (cf. iterative.cpp)
  **/
  boost::numeric::ublas::vector<ScalarType> ublas_vec, ublas_result;
  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_matrix;

  // Read matrix
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return EXIT_FAILURE;
  }

  // Set up rhs and result vector
  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", ublas_vec))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ublas_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }

  viennacl::vector<ScalarType> vcl_vec(ublas_vec.size(), ctx);
  viennacl::vector<ScalarType> vcl_result(ublas_vec.size(), ctx);
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix(ublas_vec.size(), ublas_vec.size(), ctx);

  // Copy to GPU
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);
  viennacl::copy(ublas_vec, vcl_vec);
  viennacl::copy(ublas_result, vcl_result);

  /**
  * Instantiate a tag for the conjugate gradient solver, the AMG preconditioner tag, and create an AMG preconditioner object:
  **/
  viennacl::linalg::cg_tag cg_solver;
  viennacl::linalg::amg_tag amg_tag;

  /**
  * Run solver without preconditioner. This serves as a baseline for comparison.
  * Note that iterative solvers without preconditioner on GPUs can be very efficient because they map well to the massively parallel hardware.
  **/
  std::cout << "-- CG solver (CPU, no preconditioner) --" << std::endl;
  run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, viennacl::linalg::no_precond());

  std::cout << "-- CG solver (GPU, no preconditioner) --" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, viennacl::linalg::no_precond());

  /**
  * Generate the setup for an AMG preconditioner of Ruge-Stueben type with direct interpolation (RS+DIRECT) and run the solver:
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS,       // coarsening strategy
                                      VIENNACL_AMG_INTERPOL_DIRECT, // interpolation strategy
                                      0.25, // strength of dependence threshold
                                      0.2,  // interpolation weight
                                      0.67, // jacobi smoother weight
                                      3,    // presmoothing steps
                                      3,    // postsmoothing steps
                                      0);   // number of coarse levels to be used (0: automatically use as many as reasonable)
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of Ruge-Stueben type with classic interpolation (RS+CLASSIC) and run the solver:
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS, VIENNACL_AMG_INTERPOL_CLASSIC, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg ( cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, CLASSIC INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of Ruge-Stueben type with only one pass and direct interpolation (ONEPASS+DIRECT)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_ONEPASS, VIENNACL_AMG_INTERPOL_DIRECT,0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "ONEPASS COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of parallel Ruge-Stueben type with direct interpolation (RS0+DIRECT)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS0, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS0 COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of parallel Ruge-Stueben type (with communication across domains) and use direct interpolation (RS3+DIRECT)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS3, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS3 COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner which as aggregation-based (AG)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_AG, 0.08, 0, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, AG INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner with smoothed aggregation (SA)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_SA, 0.08, 0.67, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, SA INTERPOLATION",amg_tag);

  /**
  *  That's it.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


//#define BZ_DISABLE_RESTRICT
#define BZ_ARRAY_2D_NEW_STENCIL_TILING

#include <blitz/array.h>
#include <blitz/timer.h>
#include <blitz/benchext.h>
#include <blitz/vector2.h>

#ifdef BZ_HAVE_STD
  #include <fstream>
#else
  #include <fstream.h>
#endif

BZ_USING_NAMESPACE(blitz)

#if defined(BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES)
 #define echo_f90           echo_f90_
 #define echo_f77           echo_f77_
 #define echo_f90_tuned     echo_f90_tuned_
 #define echo_f77tuned      echo_f77tuned_
#elif defined(BZ_FORTRAN_SYMBOLS_WITH_DOUBLE_TRAILING_UNDERSCORES)
 #define echo_f90           echo_f90__
 #define echo_f77           echo_f77__
 #define echo_f90_tuned     echo_f90_tuned__
 #define echo_f77tuned      echo_f77tuned__
#elif defined(BZ_FORTRAN_SYMBOLS_CAPS)
 #define echo_f90           ECHO_F90
 #define echo_f77           ECHO_F77
 #define echo_f90_tuned     ECHO_F90_TUNED
 #define echo_f77tuned      ECHO_F77TUNED
#endif

extern "C" {
void echo_f90(int& N, int& niters, float& check);
void echo_f77(int& N, int& niters, float& check);
void echo_f90_tuned(int& N, int& niters, float& check);
void echo_f77tuned(int& N, int& niters, float& check);
}

void f77(BenchmarkExt<int>&);
void f90(BenchmarkExt<int>&);
void f77_tuned(BenchmarkExt<int>&);
void f90_tuned(BenchmarkExt<int>&);

void echo_BlitzInterlacedCycled(BenchmarkExt<int>&);
void echo_BlitzCycled(BenchmarkExt<int>&);
void echo_BlitzRaw(BenchmarkExt<int>&);
void echo_BlitzStencil(BenchmarkExt<int>&);

int main()
{
    Timer timer;
    float check;
    int numBenchmarks = 6;
#ifdef FORTRAN_90
    numBenchmarks+=2;
#endif

    BenchmarkExt<int> bench("Acoustic 2D Benchmark", numBenchmarks);
    const int numSizes=7;
    bench.setNumParameters(numSizes);
    Vector<int> parameters(numSizes);
    parameters=10*pow(2.0,tensor::i);
    Vector<double> flops(numSizes);
    flops=(parameters-2)*(parameters-2) * 9.0;
    Vector<long> iters(numSizes);
    // iters must be divisible by 3 for tuned fortran versions
    iters=cast<long>(100000000/flops)*3;

    bench.setParameterVector(parameters);
    bench.setParameterDescription("Matrix size");
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);
    bench.setDependentVariable("flops");
    bench.beginBenchmarking();

    echo_BlitzRaw(bench);
    echo_BlitzStencil(bench);

#if 0
    echo_BlitzInterlaced(bench, c);
#endif

    echo_BlitzCycled(bench);
    echo_BlitzInterlacedCycled(bench);

#ifdef FORTRAN_90
    f90(bench);
    f90_tuned(bench);
#endif

    f77(bench);
    f77_tuned(bench);

    bench.endBenchmarking();
    bench.saveMatlabGraph("acoustic.m");

    return 0;
}

void checkArray(Array<float,2>& A, int N)
{
    float check = 0.0;
    for (int i=0; i < N; ++i)
        for (int j=0; j < N; ++j)
            check += ((i+1)*N + j + 1) * A(i,j);

    cout << "Array check: " << check << endl;
}

void setInitialConditions(Array<float,2>& c, Array<float,2>& P1,
    Array<float,2>& P2, Array<float,2>& P3, int N);

void echo_BlitzRaw(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (raw)");
    while (!bench.doneImplementationBenchmark())
    {
      int N = bench.getParameter();
      int niters = bench.getIterations();

    Array<float,2> P1(N,N), P2(N,N), P3(N,N), c(N,N);
    Range I(1,N-2), J(1,N-2);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J) = (2-4*c(I,J)) * P2(I,J)
          + c(I,J)*(P2(I-1,J) + P2(I+1,J) + P2(I,J-1) + P2(I,J+1))
          - P1(I,J);

        P1 = P2;
        P2 = P3;
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();

#if 0
ofstream ofs("testecho.m");
ofs << "A = [";
for (int i=0; i < N; ++i)
{
  for (int j=0; j < N; ++j)
  {
    ofs << int(8192*P2(i,j)+1024*c(i,j)) << " ";
  }
  if (i < N-1)
    ofs << ";" << endl;
}
ofs << "];" << endl;
#endif

}

void echo_BlitzCycled(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (cycled)");
    while (!bench.doneImplementationBenchmark())
    {
    int N = bench.getParameter();
    int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;

    Array<float,2> P1(N,N), P2(N,N), P3(N,N), c(N,N);
    Range I(1,N-2), J(1,N-2);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J) = (2-4*c(I,J)) * P2(I,J)
          + c(I,J)*(P2(I-1,J) + P2(I+1,J) + P2(I,J-1) + P2(I,J+1))
          - P1(I,J);

        cycleArrays(P1,P2,P3);
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();
}

void echo_BlitzInterlacedCycled(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (interlaced & cycled)");
    while (!bench.doneImplementationBenchmark())
    {
    int N = bench.getParameter();
    int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;

    Array<float,2> P1, P2, P3, c;
    allocateArrays(shape(N,N), P1, P2, P3, c);
    Range I(1,N-2), J(1,N-2);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J) = (2-4*c(I,J)) * P2(I,J)
          + c(I,J)*(P2(I-1,J) + P2(I+1,J) + P2(I,J-1) + P2(I,J+1))
          - P1(I,J);

        cycleArrays(P1,P2,P3);
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();
}

BZ_DECLARE_STENCIL4(acoustic2D,P1,P2,P3,c)
  P3 = 2 * P2 + c * Laplacian2D_stencilop(P2) - P1;
BZ_STENCIL_END

void echo_BlitzStencil(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (stencil)");
    while (!bench.doneImplementationBenchmark())
    {
    int N = bench.getParameter();
    int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;

    Array<float,2> P1, P2, P3, c;
    allocateArrays(shape(N,N), P1, P2, P3, c);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        applyStencil(acoustic2D(), P1, P2, P3, c);
        cycleArrays(P1,P2,P3);
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();
}

void setInitialConditions(Array<float,2>& c, Array<float,2>& P1,
    Array<float,2>& P2, Array<float,2>& P3, int N)
{
    // Set the velocity field
    c = 0.2;

    // Solid block with which the pulse collides
    int blockLeft = 0;
    int blockRight = int(2*N/5.0-1);
    int blockTop = int(N/3-1);
    int blockBottom = int(2*N/3.0-1);
    c(Range(blockTop,blockBottom),Range(blockLeft,blockRight)) = 0.5;

    // Channel directing the pulse leftwards
    int channelLeft = int(4*N/5.0-1);
    int channelRight = N-1;
    int channel1Height = int(3*N/8.0-1);
    int channel2Height = int(5*N/8.0-1);
    c(channel1Height,Range(channelLeft,channelRight)) = 0.0;
    c(channel2Height,Range(channelLeft,channelRight)) = 0.0;

    // Initial pressure distribution: gaussian pulse inside the channel
    BZ_USING_NAMESPACE(blitz::tensor)
    int cr = int(N/2-1);
    int cc = int(7.0*N/8.0-1);
    // pow2 is not defined for pod types.
    float s2 = 64.0 * 9.0 / pow(N/2.0,2);
    cout << "cr = " << cr << " cc = " << cc << " s2 = " << s2 << endl;
    P1 = 0.0;
    P2 = exp(-(pow2(i-cr)+pow2(j-cc)) * s2);
    P3 = 0.0;
}

void f77(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran77");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f77(N, niters, check);
  bench.stop();
    cout << check << endl;
    }
    bench.endImplementation();
};

void f77_tuned(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran77 (tuned)");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f77tuned(N, niters, check);
  bench.stop();
    cout << check << endl;
    }

    bench.endImplementation();
};

void f90(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran90");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f90(N, niters, check);
  bench.stop();
    cout << check << endl;
    }

    bench.endImplementation();
};
void f90_tuned(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran90 (tuned)");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f90_tuned(N, niters, check);
  bench.stop();
    cout << check << endl;
    }

    bench.endImplementation();
};


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/filters/filter.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <vector>
#include <fstream>

using Point = pcl::PointXYZI;
using PointCloud = pcl::PointCloud<Point>;

void ros_callback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    PointCloud::Ptr pc(new PointCloud);
    std::vector<int> indices;
    pcl::KdTreeFLANN<Point> kdtree; // 用于搜索最近点

    // sensor_msgs::PointCloud2 -> pcl::PointCloud
    pcl::fromROSMsg(*msg, *pc);

    // is_dense 用于表示点云中的所有数据是否合法
    // 设置为 否，让后续的函数再检查一遍，把 Nan（not a number）
    // 这种不合法的数值全部去掉
    pc->is_dense = false;
    pcl::removeNaNFromPointCloud(*pc, *pc, indices);

    // 初始化 kd 树
    kdtree.setInputCloud(pc);

    // 提前将需要在循环中用到的变量初始化好，放置在循环中重复构造变量与析构，拖慢程序运行速度
    const int k = 20;                           // 临近点数量，根据作业要求设置为 20
    std::vector<int> point_idx(k);              // 用来保存临近点再原来点云中的下标
    std::vector<float> point_sq_dis(k);         // 用来保存临近点到目标点距离的平方
    std::vector<float> features(6);             // 用来保存六种点云特征
    std::vector<float> e(3);                    // 用来保存 k+1 个点经过 PCA 分析后得到的三个特征值计算得到的 e，从大到小排序
    std::ofstream file;                         // 输出计算结果的目标文件
    Eigen::Matrix<float, 3, 21> nearest_points; // 3x(k+1) 维的矩阵，用来保存点云中的点
    Eigen::Matrix3f covariance;                 // 用来保存协方差矩阵
    Eigen::Vector3f m, eigen_value;             // m 为 k+1 个点的质心，eigen_value 用来保存计算好的计算好的特征值

    // 打开文件，没有就凭空创建一个，如果有就删掉里面的内容，再写入新的
    // 一般不会出错
    file.open("wcm.txt");

    // pcl::PointCloud 中保存点的对象，我们用引用单独给他拿出来
    // 方便后续写代码
    auto& points = pc->points;
    for (size_t i = 0; i < pc->size(); i++)
    {
        // 每隔五个点计算一次特征值，作业没有要求这么做
        // 只是想这么做，希望能快点
        if(i%5 != 0) continue;

        // 重置 m，因为 m 需要累加，而其他的变量只需要赋值
        m = m.Zero();

        // 搜索目标点最近的几个点
        // https://pointclouds.org/documentation/classpcl_1_1_organized_neighbor_search.html#a3c18f38a4aad5fe6c05179906faf14cb
        kdtree.nearestKSearch(points[i], k, point_idx, point_sq_dis);

        // 累加搜索后的数据
        for (size_t j = 0; j < k; j++)
        {
            // 矩阵的块操作，将每个点作为列向量存入 nearest_points
            // http://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
            nearest_points.col(j) << points[point_idx[j]].x, points[point_idx[j]].y, points[point_idx[j]].z;
            m[0] += points[point_idx[j]].x;
            m[1] += points[point_idx[j]].y;
            m[2] += points[point_idx[j]].z;
        }
        nearest_points.col(k) << points[i].x, points[i].y, points[i].z;
        m[0] += points[i].x;
        m[1] += points[i].y;
        m[2] += points[i].z;

        // 矩阵的广播操作，将每一列减去 k+1 个点的质心
        // http://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
        nearest_points.colwise() -= (m/(k+1));

        // 计算协方差矩阵
        covariance = nearest_points * nearest_points.transpose();

        // 对称矩阵求特征值
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(covariance);
        eigen_value = solver.eigenvalues();

        // 矩阵的 reduction 操作，计算矩阵所有元素的和
        // http://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
        e[0] = eigen_value[2]/eigen_value.sum();
        e[1] = eigen_value[1]/eigen_value.sum();
        e[2] = eigen_value[0]/eigen_value.sum();

        // 计算点云特征
        features[0] = (e[0]-e[1])/e[0];
        features[1] = (e[1]-e[2])/e[0];
        features[2] = e[2]/e[0];
        features[3] = std::cbrt(std::accumulate(e.begin(), e.end(), 0))*3.0f;
        features[4] = -e[0] * std::log(e[0]) - e[1] * std::log(e[1]) - e[2] * std::log(e[2]);
        features[5] = 3.0f*e[2];

        // 将结果写入文件，空格分开，最有追加一个换行
        // 这种特殊的换行有清空缓冲区的效果
        for(auto& num : features)
            file << num << " ";
        file << std::endl;
    }
    // 关闭文件
    file.close();

    std::cout << "cal done" << std::endl;

    // 关闭节点
    // 下面是官方描述
    // Disconnects everything and unregisters from the master.
    // It is generally not necessary to call this function,
    // as the node will automatically shutdown when all NodeHandles destruct.
    // However, if you want to break out of a spin() loop explicitly, this function allows that.
    ros::shutdown();
    return;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "aiimooc_wcm_node");
    ros::NodeHandle nh("~");

    auto sub_rslidar = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_points", 2, ros_callback);

    ros::spin();
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/scalar/fast_rsqrt.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/four.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/include/constants/sqrt_2.hpp>

#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>

NT2_TEST_CASE_TPL( fast_rsqrt, BOOST_SIMD_REAL_TYPES )
{
  using boost::simd::fast_rsqrt;
  using boost::simd::tag::fast_rsqrt_;

  // return type conformity test
  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<fast_rsqrt_(T)>::type
                  , T
                  );

  // specific values tests
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::Mone<T>()), boost::simd::Nan<T>(), 75);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::Nan<T>()), boost::simd::Nan<T>(), 0.5);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::One<T>()), boost::simd::One<T>(), 30);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::Four<T>()), boost::simd::Half<T>(), 30);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(T(0.5)), boost::simd::Sqrt_2<T>(), 70);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(T(0.01)), T(10), 30);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(T(0.0001)), T(100), 30);
}


#include "../kernels.h"
#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <time.h>

namespace gil = boost::gil;

void fill_gil_view_with_opencv_mat(cv::Mat opencv_mat,
                                   gil::gray8_view_t gil_view) {
  for (int i = 0; i < opencv_mat.rows; i++)
    for (int j = 0; j < opencv_mat.cols; j++)
      gil_view(j, i) = opencv_mat.at<uchar>(i, j);
}

int main(int argc, char *argv[]) {
  // Read input image using opencv's imread()
  cv::Mat opencv_image = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);

  // Declare input image
  gil::gray8_image_t image(opencv_image.cols, opencv_image.rows);

  // Fill GIL image view with image read using opencv's imread()
  fill_gil_view_with_opencv_mat(opencv_image, gil::view(image));

  // Declare output image
  gil::gray8_image_t output(image.dimensions());

  // Create a 2D GIL kernel
  gil::detail::kernel_2d<float> kernel(sobel3x3KernelAlign, 9, 1, 1);

  clock_t start, end;
  start = clock();
  // Apply 2D convolution between input image and kernel
  gil::detail::convolve_2d(gil::view(image), kernel, gil::view(output));
  end = clock();
  std::cout << "Execution time: " << (double)(end - start) / CLOCKS_PER_SEC
            << " s" << std::endl;

  // Save obtained image
  gil::write_view(argv[2], gil::view(output), gil::png_tag{});
}


// Copyright David Abrahams 2002. Permission to copy, use,
// modify, sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided
// "as is" without express or implied warranty, and with no claim as
// to its suitability for any purpose.
#include <boost/python/operators.hpp>
#include <boost/python/class.hpp>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <string>
#include "test_class.hpp"
#if __GNUC__ != 2
# include <ostream>
#else
# include <ostream.h>
#endif

// Just use math.h here; trying to use std::pow() causes too much
// trouble for non-conforming compilers and libraries.
#include <math.h>

using namespace boost::python;

struct X : test_class<>
{
    typedef test_class<> base_t;

    X(int x) : base_t(x) {}
    X const operator+(X const& r) const { return X(value() + r.value()); }
};

X operator-(X const& l, X const& r) { return X(l.value() - r.value()); }
X operator-(int l, X const& r) { return X(l - r.value()); }
X operator-(X const& l, int r) { return X(l.value() - r); }

X operator-(X const& x) { return X(-x.value()); }

X& operator-=(X& l, X const& r) { l.set(l.value() - r.value()); return l; }

bool operator<(X const& x, X const& y) { return x.value() < y.value(); }
bool operator<(X const& x, int y) { return x.value() < y; }
bool operator<(int x, X const& y) { return x < y.value(); }

X abs(X x) { return X(x.value() < 0 ? -x.value() : x.value()); }

X pow(X x, int y)
{
    return X(int(pow(double(x.value()), double(y))));
}

X pow(X x, X y)
{
    return X(int(pow(double(x.value()), double(y.value()))));
}

int pow(int x, X y)
{
    return int(pow(double(x), double(y.value())));
}

std::ostream& operator<<(std::ostream& s, X const& x)
{
    return s << x.value();
}

BOOST_PYTHON_MODULE(operators_ext)
{
    class_<X>("X", init<int>())
        .def("value", &X::value)
        .def(self + self)
        .def(self - self)
        .def(self - int())
        .def(other<int>() - self)
        .def(-self)
        .def(self < other<int>())
        .def(self < self)
        .def(1 < self)
        .def(self -= self)
        .def(abs(self))
        .def(str(self))

        .def(pow(self,self))
        .def(pow(self,int()))
        .def(pow(int(),self))
        ;

    class_<test_class<1> >("Z", init<int>())
        .def(int_(self))
        .def(float_(self))
        .def(complex_(self))
        ;
}

#include "module_tail.cpp"


/*
 * example-rtc.cpp
 *
 * Copyright (c) 2010 Marc Kirchner
 *               2011 David Sichau
 *
 */

#include <libpipe/config.hpp>
#include <stdlib.h>
#include <exception>
#include <iostream>
#include <set>
#include <boost/pointer_cast.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <libpipe/rtc/Algorithm.hpp>
#include <libpipe/rtc/Filter.hpp>
#include <libpipe/rtc/Manager.hpp>
#include <libpipe/rtc/ManagerFactory.hpp>
#include <libpipe/rtc/AlgorithmFactory.hpp>
#include <libpipe/utilities/Exception.hpp>
#include <libpipe/Request.hpp>
#include <libpipe/rtc/SharedData.hpp>
#include <libpipe/rtc/PipelineLoader.hpp>

#include "walltime.h"

int MATRIX_SIZE = 100;

/** Simple Matrix Multiplication Algorithm Example.
 */
class MatrixMulAlgorithm : public libpipe::rtc::Algorithm
{
public:
    // use convenience typedefs to avoid cluttering up the code
    typedef std::vector<double> Doubles;
    typedef libpipe::rtc::SharedData<Doubles> SharedDoubles;

    /** Virtual constructor.
     * @return Base class pointer to a new \c MatrixMulAlgorithm object.
     */
    static Algorithm* create()
    {
        return new MatrixMulAlgorithm;
    }

    /** Destructor.
     */
    virtual ~MatrixMulAlgorithm()
    {
    }

    /** Executes the algorithm and updates the output data.
     * This is where the algorithm implementation needs to go.
     * @param[in,out] req The request object, forwarded from \c process request.
     */
    void update(libpipe::Request& req)
    {
        // Take note of what we are doing
        LIBPIPE_PIPELINE_TRACE("MatrixMulAlgorithm::update: start.");

        // Gain access to the in- and output
        LIBPIPE_PREPARE_READ_ACCESS(input1_, tempIn1, Doubles, "MatrixIn1");
        LIBPIPE_PREPARE_READ_ACCESS(input2_, tempIn2, Doubles, "MatrixIn2");
        LIBPIPE_PREPARE_WRITE_ACCESS(output_, tempOut, Doubles, "MatrixOut");
        // Again, log what is happening
        LIBPIPE_PIPELINE_TRACE(
            "MatrixMulAlgorithm::update: multiplication of two matrices.");

        // Let's roll. This is O(N^2).
        for (int i = 0; i < MATRIX_SIZE; i++) {
            for (int j = 0; j < MATRIX_SIZE; j++) {
                double sum = 0;
                for (int k = 0; k < MATRIX_SIZE; k++) {
                    sum += tempIn1[i * MATRIX_SIZE + k] * tempIn2[k
                            * MATRIX_SIZE + j];
                }
                tempOut[i * MATRIX_SIZE + j] = sum / MATRIX_SIZE;
            }
        }

        LIBPIPE_CLEAR_ACCESS(output_);
        LIBPIPE_CLEAR_ACCESS(input1_);
        LIBPIPE_CLEAR_ACCESS(input2_);

        // And tell the world that we are done.
        LIBPIPE_PIPELINE_TRACE("MatrixMulAlgorithm::update: end.");
    }

protected:
    /** Constructor.
     * Make sure to call the \c libpipe::rtc::Algorithm constructor.
     */
    MatrixMulAlgorithm() :
        libpipe::rtc::Algorithm()
    {
        ports_["MatrixIn1"] = boost::make_shared<SharedDoubles>();
        ports_["MatrixIn2"] = boost::make_shared<SharedDoubles>();
        ports_["MatrixOut"] = boost::make_shared<SharedDoubles>(
            new Doubles(MATRIX_SIZE * MATRIX_SIZE));
    }

private:

    /** Registers the Algorithm with the factory.
     * @return A boolean that indicates if the registration was successful
     */
    static const bool registerLoader()
    {
        std::string ids = "MatrixMulAlgorithm";
        return libpipe::rtc::AlgorithmFactory::instance().registerType(ids,
            MatrixMulAlgorithm::create);
    }
    /// if true then class is registered w/ Algorithm Factory
    static const bool registered_;

};

const bool MatrixMulAlgorithm::registered_ =
        MatrixMulAlgorithm::registerLoader();

/** Provides a constant matrix as output.
 * This is an example of a 'source': the \c Source algorithm does not require
 * any input and will always provide a predefined matrix on its output port.
 */
class Source : public libpipe::rtc::Algorithm
{
public:
    // use convenience typedefs to avoid cluttering up the code
    typedef std::vector<double> Doubles;
    typedef libpipe::rtc::SharedData<Doubles> SharedDoubles;

    static Algorithm* create()
    {
        return new Source;
    }

    /** Destructor.
     */
    virtual ~Source()
    {
    }

    /** Updates the output data (i.e. does nothing).
     * The output is provided as a constant, hence there is nothing to do.
     * @param[in] req The request object.
     */
    void update(libpipe::Request& req)
    {
        LIBPIPE_PIPELINE_TRACE("Source::update: start.");
        LIBPIPE_PREPARE_WRITE_ACCESS(output_, tempOut, Doubles, "MatrixOut");

        // fill matrix with some values
        LIBPIPE_PIPELINE_TRACE("Source::update: filling matrix.");
        for (int row = 0; row < MATRIX_SIZE; row++) {
            for (int col = 0; col < MATRIX_SIZE; col++) {
                tempOut[col + (row * MATRIX_SIZE)] = col + row;
            }
        }

        LIBPIPE_CLEAR_ACCESS(output_)
        LIBPIPE_PIPELINE_TRACE("Source::update: end.");
    }

private:
    /** Constructor.
     */
    Source() :
        libpipe::rtc::Algorithm()
    {
        ports_["MatrixOut"] = boost::make_shared<SharedDoubles>(
            new Doubles(MATRIX_SIZE * MATRIX_SIZE));
    }
    /** registers the Algorithm in the factory
     * @return true is registration was successful
     */
    static const bool registerLoader()
    {
        std::string ids = "Source";
        return libpipe::rtc::AlgorithmFactory::instance().registerType(ids,
            Source::create);
    }
    /// true is class is registered in Algorithm Factory
    static const bool registered_;

};

const bool Source::registered_ = Source::registerLoader();

/** Prints the Matrix
 */
class Printer : public libpipe::rtc::Algorithm
{
public:
    // use convenience typedefs to avoid cluttering up the code
    typedef std::vector<double> Doubles;
    typedef libpipe::rtc::SharedData<Doubles> SharedDoubles;

    static Algorithm* create()
    {
        return new Printer;
    }

    /** Destructor.
     */
    virtual ~Printer()
    {
    }

    /** Updates the output data (i.e. does nothing).
     * The output is provided as a constant, hence there is nothing to do.
     * @param[in] req The request object.
     */
    void update(libpipe::Request& req)
    {
        LIBPIPE_PIPELINE_TRACE( "Printer::update: start.");
        // Gain access to the in- and output
        LIBPIPE_PREPARE_READ_ACCESS(input_, in, Doubles, "MatrixIn");

        LIBPIPE_PIPELINE_TRACE("printing matrix");

        // print the matrix
        for (int row = 0; row < MATRIX_SIZE; row++) {
            for (int col = 0; col < MATRIX_SIZE; col++) {
                std::cout << in[col + (row * MATRIX_SIZE)] << " ";
            }
            std::cout << '\n';
        }
        std::cout << '\n' << std::endl;
        LIBPIPE_CLEAR_ACCESS(input_);
        LIBPIPE_PIPELINE_TRACE("Printer::update: end.");
    }

protected:

private:
    /** Constructor.
     */
    Printer() :
        libpipe::rtc::Algorithm()
    {
        ports_["MatrixIn"] = boost::make_shared<SharedDoubles>();
    }
    /** registers the Algorithm in the factory
     * @return true is registration was successful
     */
    static const bool registerLoader()
    {
        std::string ids = "Printer";
        return libpipe::rtc::AlgorithmFactory::instance().registerType(ids,
            Printer::create);
    }
    /// true is class is registered in Algorithm Factory
    static const bool registered_;

};

const bool Printer::registered_ = Printer::registerLoader();

/** Handles the several Algorithms so that they can be executed in parallel,
 *  as the Pipeline will execute them in sequential order.
 */
class Handler : public libpipe::rtc::Algorithm
{
public:
    static Algorithm* create()
    {
        return new Handler;
    }

    /** Destructor.
     */
    virtual ~Handler()
    {
    }

    /** Does nothing.
     * The output is provided as a constant, hence there is nothing to do.
     * @param[in] req The request object.
     */
    void update(libpipe::Request& req)
    {
        LIBPIPE_PIPELINE_TRACE("start handler");
    }

protected:

private:
    /** Constructor.
     */
    Handler() :
        libpipe::rtc::Algorithm()
    {
    }
    /** registers the Algorithm in the factory
     * @return true is registration was successful
     */
    static const bool registerLoader()
    {
        std::string ids = "Handler";
        return libpipe::rtc::AlgorithmFactory::instance().registerType(ids,
            Handler::create);
    }
    /// true is class is registered in Algorithm Factory
    static const bool registered_;

};

const bool Handler::registered_ = registerLoader();

int main(int argc, char *argv[])
{
    using namespace libpipe::rtc;
    if (argc == 2) {
        MATRIX_SIZE = atoi(argv[1]);
    } else {
        std::cerr << "usage: ./example-matrices MATRIX_SIZE[int]" << std::endl;
        exit(1);
    }

    std::cout << "Matrix Size: " << MATRIX_SIZE << std::endl;

    std::map < std::string, std::string > inputFiles;
    inputFiles["FilterInput"] = "inputFileFilterJSONMatrix.txt";
    inputFiles["ConnectionInput"] = "inputFileConnectionJSONMatrix.txt";
    inputFiles["PipelineInput"] = "inputFilePipelineJSONMatrix.txt";
    inputFiles["ParameterInput"] = "inputFileParametersJSONMatrix.txt";

    Pipeline pipeline;
    try {
        PipelineLoader loader(inputFiles);
        pipeline = loader.getPipeline();
    } catch (libpipe::utilities::Exception& e) {
        std::cerr << e.what() << std::endl;
    }

    double time, time_start = 0.0;

    time = walltime(&time_start);

    try {
        pipeline.run();
    } catch (libpipe::utilities::Exception& e) {
        std::cerr << e.what() << std::endl;
    }

    time = walltime(&time);

    std::cout << time << " sec" << std::endl;

    std::vector < std::string > trace;
    trace = pipeline.getTrace();
    for (std::vector<std::string>::const_iterator i = trace.begin(); i
            != trace.end(); ++i) {
        std::cout << *i << '\n';
    }

    std::cout
            << "All output after this is due to automatically called destructors."
            << std::endl;

    return EXIT_SUCCESS;

}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "DistributionFunctions.h"

#include "Pars.h"

#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/factorials.hpp>

using namespace cppqedutils;
using namespace boost::math;

namespace quantumdata {

ParsFunctionScan::ParsFunctionScan(parameters::Table& p, const std::string& mod)
  : fLimitXUL(p.addTitle("Distribution function scan",mod).add("fLimitXUL","",2.)),
    fLimitYUL(p.add("fLimitYUL","",2.)),
    fLimitXL(p.add("fLimitXL","",-2.)),
    fLimitXU(p.add("fLimitXU","",2.)),
    fLimitYL(p.add("fLimitYL","",-2.)),
    fLimitYU(p.add("fLimitYU","",2.)),
    fStep(p.add("fStep","",.1)),
    fCutoff(p.add("fCutoff","",100))
{}

namespace {

const WignerFunctionKernelOld::Hermites fillWithHermite(size_t dim, double x)
{
  WignerFunctionKernelOld::Hermites res(2*dim-1);
  res(0)=hermite(0,x); res(1)=hermite(1,x);
  for (unsigned l=1; l<res.size()-1; ++l)
    res(l+1)=hermite_next(l,x,res(l),res(l-1));
  return res;
}

}

double details::w(size_t n, double r, size_t k)
{
  const double sqrR=sqr(r);
  return minusOneToThePowerOf(n)/PI*sqrt(factorial<double>(n)/factorial<double>(n+k))*exp(-2*sqrR)*pow(2*r,k)*laguerre(n,k,4*sqrR);
}

WignerFunctionKernelOld::WignerFunctionKernelOld(double x, double y, size_t dim)
  : hermite_m2x_(fillWithHermite(dim,-2*x)), hermite_2y_(fillWithHermite(dim,2*y))
{}

dcomp WignerFunctionKernelOld::operator()(size_t m, size_t n) const
{
  dcomp res(0);

  for (size_t u=0; u<=m; ++u) for (size_t v=0; v<=n; ++v)
    res+=
      binomial_coefficient<double>(m,u)*
      binomial_coefficient<double>(n,v)*
      minusOneToThePowerOf(v)*
      pow(DCOMP_I,u+v)*
      hermite_m2x_(    u+v)*
      hermite_2y_ (n+m-u-v);

  return res;

}

} // quantumdata


//  justanhduc
// Oct 2020

#include <Eigen/Core>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "TSDFVolumeCPU.hpp"
#include "Raycaster.hpp"
#include "RenderUtilities.hpp"

#define MAX_PATH 260  // for linux

int main() {
    using namespace Eigen;

    const int dim_x = 101, dim_y = 139, dim_z = 106;
    const int n_max_frame = 1;
    auto *tsdf = new float[dim_x * dim_y * dim_z * n_max_frame];
    char stmp[MAX_PATH];
    float ftemp[8];
    for (int i = 0; i < n_max_frame; i++) {
        sprintf(stmp, "../volume/_tsdf_multi_%03d.bin", i);  // for linux
        FILE *fp = nullptr;
        fp = fopen(stmp, "rb");  // for linux
        if (fp == 0) {
            printf("Cannot read file %s\n", stmp);
            exit(-1);
        }
        fread(ftemp, sizeof(float), 8, fp);
        fread(tsdf + (dim_x * dim_y * dim_z) * i, dim_x * dim_y * dim_z, sizeof(float), fp);
        fclose(fp);
    }

    TSDFVolumeCPU volume(dim_x, dim_y, dim_z, dim_x * .01, dim_y * .01, dim_z * .01);
    volume.set_distance_data(tsdf);
    volume.set_truncation_distance(.09);
    std::cout << "Read file. Rendering." << std::endl;

    uint16_t width = 1024;
    uint16_t height = 768;

    Eigen::Matrix<float, 3, Eigen::Dynamic> vertices;
    Eigen::Matrix<float, 3, Eigen::Dynamic> normals;

    Vector3f light_source{0, 3, 3};
    auto eye = Vector3f{0.5, 0, 2};  // view point
    Camera cam((float) width / 2, (float) height / 2, (float) (width - 1) / 2,
               (float) (height - 1) / 2);
    cam.move_to(eye);
    cam.look_at(0.5, 0.5, 0.5);

    Raycaster r{width, height};
    r.raycast(volume, cam, vertices, normals);
    uint8_t *scene = render_scene(width, height, vertices, normals, cam, light_source);

    cv::Mat frame(height, width, 0, scene);
    cv::imshow("Frame", frame);
    cv::waitKey(0);
    return 0;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     visibility_graph_feature.cpp
 * \author   Collin Johnson
 *
 * Definition of VisibilityGraphFeature.
 */

#include "utils/visibility_graph_feature.h"
#include <algorithm>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

namespace vulcan
{
namespace utils
{

std::string feature_type_to_string(VisibilityGraphFeatureType type)
{
    switch (type) {
    case VisibilityGraphFeatureType::none:
        return "none";

    case VisibilityGraphFeatureType::mean_edge_count:
        return "mean edge count";

    case VisibilityGraphFeatureType::clustering_coeff:
        return "clustering coeff";

    case VisibilityGraphFeatureType::degree_centrality:
        return "degree centrality";

    case VisibilityGraphFeatureType::closeness_centrality:
        return "closeness centrality";

    case VisibilityGraphFeatureType::betweenness_centrality:
        return "betweenness centrality";

    case VisibilityGraphFeatureType::pagerank:
        return "pagerank";

    case VisibilityGraphFeatureType::num_features:
    default:
        break;
    }

    return "";
}

VisibilityGraphFeature::VisibilityGraphFeature(void) : type_(VisibilityGraphFeatureType::none)
{
}

VisibilityGraphFeature::VisibilityGraphFeature(VisibilityGraphFeatureType type, const std::vector<value_type>& values)
: type_(type)
, values_(values)
{
}

feature_stats_t VisibilityGraphFeature::stats(std::vector<VisGraphVertex>::const_iterator begin,
                                              std::vector<VisGraphVertex>::const_iterator end) const
{
    using namespace boost::accumulators;
    accumulator_set<double, boost::accumulators::stats<tag::mean, tag::variance, tag::min, tag::max>> statsAcc;

    for (auto& vertToValue : values_) {
        if ((begin == end) || (std::find(begin, end, vertToValue.first) != end)) {
            statsAcc(vertToValue.second);
        }
    }

    return {min(statsAcc), mean(statsAcc), max(statsAcc), std::sqrt(variance(statsAcc))};
}

}   // namespace utils
}   // namespace vulcan