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

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

 * See LICENSE for the license information

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

/**
 * @file    LevenbergMarquardtOptimizer.cpp
 * @brief
 * @author  Richard Roberts
 * @author  Luca Carlone
 * @date  Feb 26, 2012
 */

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Errors.h>

#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/map.hpp>
#include <string>
#include <cmath>
#include <fstream>

using namespace std;

namespace gtsam {

using boost::adaptors::map_values;

/* ************************************************************************* */
LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(
    const std::string &src) const {
  std::string s = src;
  boost::algorithm::to_upper(s);
  if (s == "SILENT")
    return LevenbergMarquardtParams::SILENT;
  if (s == "LAMBDA")
    return LevenbergMarquardtParams::LAMBDA;
  if (s == "TRYLAMBDA")
    return LevenbergMarquardtParams::TRYLAMBDA;
  if (s == "TRYCONFIG")
    return LevenbergMarquardtParams::TRYCONFIG;
  if (s == "TRYDELTA")
    return LevenbergMarquardtParams::TRYDELTA;
  if (s == "DAMPED")
    return LevenbergMarquardtParams::DAMPED;

  /* default is silent */
  return LevenbergMarquardtParams::SILENT;
}

/* ************************************************************************* */
std::string LevenbergMarquardtParams::verbosityLMTranslator(
    VerbosityLM value) const {
  std::string s;
  switch (value) {
  case LevenbergMarquardtParams::SILENT:
    s = "SILENT";
    break;
  case LevenbergMarquardtParams::TERMINATION:
    s = "TERMINATION";
    break;
  case LevenbergMarquardtParams::LAMBDA:
    s = "LAMBDA";
    break;
  case LevenbergMarquardtParams::TRYLAMBDA:
    s = "TRYLAMBDA";
    break;
  case LevenbergMarquardtParams::TRYCONFIG:
    s = "TRYCONFIG";
    break;
  case LevenbergMarquardtParams::TRYDELTA:
    s = "TRYDELTA";
    break;
  case LevenbergMarquardtParams::DAMPED:
    s = "DAMPED";
    break;
  default:
    s = "UNDEFINED";
    break;
  }
  return s;
}

/* ************************************************************************* */
void LevenbergMarquardtParams::print(const std::string& str) const {
  NonlinearOptimizerParams::print(str);
  std::cout << "              lambdaInitial: " << lambdaInitial << "\n";
  std::cout << "               lambdaFactor: " << lambdaFactor << "\n";
  std::cout << "           lambdaUpperBound: " << lambdaUpperBound << "\n";
  std::cout << "           lambdaLowerBound: " << lambdaLowerBound << "\n";
  std::cout << "           minModelFidelity: " << minModelFidelity << "\n";
  std::cout << "            diagonalDamping: " << diagonalDamping << "\n";
  std::cout << "               min_diagonal: " << min_diagonal_ << "\n";
  std::cout << "               max_diagonal: " << max_diagonal_ << "\n";
  std::cout << "                verbosityLM: "
      << verbosityLMTranslator(verbosityLM) << "\n";
  std::cout.flush();
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const {
  return graph_.linearize(state_.values);
}

/* ************************************************************************* */
void LevenbergMarquardtOptimizer::increaseLambda() {
  if (params_.useFixedLambdaFactor_) {
    state_.lambda *= params_.lambdaFactor;
  } else {
    state_.lambda *= params_.lambdaFactor;
    params_.lambdaFactor *= 2.0;
  }
  params_.reuse_diagonal_ = true;
}

/* ************************************************************************* */
void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) {

  if (params_.useFixedLambdaFactor_) {
    state_.lambda /= params_.lambdaFactor;
  } else {
    // CHECK_GT(step_quality, 0.0);
    state_.lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3));
    params_.lambdaFactor = 2.0;
  }
  state_.lambda = std::max(params_.lambdaLowerBound, state_.lambda);
  params_.reuse_diagonal_ = false;

}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
    const GaussianFactorGraph& linear) {

  gttic(damp);
  if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED)
    cout << "building damped system with lambda " << state_.lambda << endl;

  // Only retrieve diagonal vector when reuse_diagonal = false
  if (params_.diagonalDamping && params_.reuse_diagonal_ == false) {
    state_.hessianDiagonal = linear.hessianDiagonal();
    BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) {
      for (int aa = 0; aa < v.size(); aa++) {
        v(aa) = std::min(std::max(v(aa), params_.min_diagonal_),
            params_.max_diagonal_);
        v(aa) = sqrt(v(aa));
      }
    }
  } // reuse diagonal

  // for each of the variables, add a prior
  double sigma = 1.0 / std::sqrt(state_.lambda);
  GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr();
  GaussianFactorGraph &damped = (*dampedPtr);
  damped.reserve(damped.size() + state_.values.size());
  if (params_.diagonalDamping) {
    BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) {
      // Fill in the diagonal of A with diag(hessian)
      try {
        Matrix A = Eigen::DiagonalMatrix<double, Eigen::Dynamic>(
            state_.hessianDiagonal.at(key_vector.first));
        size_t dim = key_vector.second.size();
        Vector b = Vector::Zero(dim);
        SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
        damped += boost::make_shared<JacobianFactor>(key_vector.first, A, b,
            model);
      } catch (std::exception e) {
        // Don't attempt any damping if no key found in diagonal
        continue;
      }
    }
  } else {
    // Straightforward damping:
    BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
      size_t dim = key_value.value.dim();
      Matrix A = Matrix::Identity(dim, dim);
      Vector b = Vector::Zero(dim);
      SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
      damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
    }
  }
  gttoc(damp);
  return dampedPtr;
}

/* ************************************************************************* */
// Log current error/lambda to file
inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
  if (!params_.logFile.empty()) {
    ofstream os(params_.logFile.c_str(), ios::app);
    boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
    os << /*inner iterations*/ state_.totalNumberInnerIterations << ","
        << 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
        << /*current error*/ currentError << "," << state_.lambda << ","
        << /*outer iterations*/ state_.iterations << endl;
  }
}

/* ************************************************************************* */
void LevenbergMarquardtOptimizer::iterate() {

  gttic(LM_iterate);

  // Pull out parameters we'll use
  const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
  const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM;

  // Linearize graph
  if (lmVerbosity >= LevenbergMarquardtParams::DAMPED)
    cout << "linearizing = " << endl;
  GaussianFactorGraph::shared_ptr linear = linearize();

  if(state_.totalNumberInnerIterations==0) // write initial error
    writeLogFile(state_.error);

  // Keep increasing lambda until we make make progress
  while (true) {

    if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
      cout << "trying lambda = " << state_.lambda << endl;

    // Build damped system for this lambda (adds prior factors that make it like gradient descent)
    GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear);
    GaussianFactorGraph &dampedSystem = (*dampedSystemPtr);

    // Try solving
    double modelFidelity = 0.0;
    bool step_is_successful = false;
    bool stopSearchingLambda = false;
    double newError;
    Values newValues;
    VectorValues delta;

    bool systemSolvedSuccessfully;
    try {
      delta = solve(dampedSystem, state_.values, params_);
      systemSolvedSuccessfully = true;
    } catch (IndeterminantLinearSystemException) {
      systemSolvedSuccessfully = false;
    }

    if (systemSolvedSuccessfully) {
      params_.reuse_diagonal_ = true;

      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
        cout << "linear delta norm = " << delta.norm() << endl;
      if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA)
        delta.print("delta");

      // cost change in the linearized system (old - new)
      double newlinearizedError = linear->error(delta);

      double linearizedCostChange = state_.error - newlinearizedError;
      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
              cout << "newlinearizedError = " << newlinearizedError <<
              "  linearizedCostChange = " << linearizedCostChange << endl;

      if (linearizedCostChange >= 0) { // step is valid
        // update values
        gttic(retract);
        newValues = state_.values.retract(delta);
        gttoc(retract);

        // compute new error
        gttic(compute_error);
        if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
          cout << "calculating error:" << endl;
        newError = graph_.error(newValues);
        gttoc(compute_error);

        if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
          cout << "old error (" << state_.error
              << ") new (tentative) error (" << newError << ")" << endl;

        // cost change in the original, nonlinear system (old - new)
        double costChange = state_.error - newError;

        if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition
          // fidelity of linearized model VS original system between
          modelFidelity = costChange / linearizedCostChange;
          // if we decrease the error in the nonlinear system and modelFidelity is above threshold
          step_is_successful = modelFidelity > params_.minModelFidelity;
          if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
            cout << "modelFidelity: " << modelFidelity << endl;
        } // else we consider the step non successful and we either increase lambda or stop if error change is small

        double minAbsoluteTolerance = params_.relativeErrorTol * state_.error;
        // if the change is small we terminate
        if (fabs(costChange) < minAbsoluteTolerance){
          if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
                        cout << "fabs(costChange)="<<fabs(costChange) << "  minAbsoluteTolerance="<< minAbsoluteTolerance
                        << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl;
          stopSearchingLambda = true;
        }
      }
    }

    ++state_.totalNumberInnerIterations;

    if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity
      state_.values.swap(newValues);
      state_.error = newError;
      decreaseLambda(modelFidelity);
      writeLogFile(state_.error);
      break;
    } else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost
      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
        cout << "increasing lambda" << endl;
      increaseLambda();
      writeLogFile(state_.error);

      // check if lambda is too big
      if (state_.lambda >= params_.lambdaUpperBound) {
        if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION)
          cout << "Warning:  Levenberg-Marquardt giving up because "
              "cannot decrease error with maximum lambda" << endl;
        break;
      }
    } else { // the change in the cost is very small and it is not worth trying bigger lambdas
      writeLogFile(state_.error);
      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
              cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
      break;
    }
  } // end while

  // Increment the iteration counter
  ++state_.iterations;
}

/* ************************************************************************* */
LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
    LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const {
  if (!params.ordering)
    params.ordering = Ordering::COLAMD(graph);
  return params;
}

} /* namespace gtsam */


//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

#ifndef BOOST_UBLAS_TENSOR_OPERATORS_COMPARISON_HPP
#define BOOST_UBLAS_TENSOR_OPERATORS_COMPARISON_HPP

#include <boost/numeric/ublas/tensor/expression.hpp>
#include <boost/numeric/ublas/tensor/expression_evaluation.hpp>
#include <type_traits>
#include <functional>

namespace boost::numeric::ublas {
template<class element_type, class storage_format, class storage_type>
class tensor;
}

namespace boost::numeric::ublas::detail {

template<class T, class F, class A, class BinaryPred>
bool compare(tensor<T,F,A> const& lhs, tensor<T,F,A> const& rhs, BinaryPred pred)
{

	if(lhs.extents() != rhs.extents()){
		if constexpr(!std::is_same<BinaryPred,std::equal_to<>>::value && !std::is_same<BinaryPred,std::not_equal_to<>>::value)
			throw std::runtime_error("Error in boost::numeric::ublas::detail::compare: cannot compare tensors with different shapes.");
		else
			return false;
	}

	if constexpr(std::is_same<BinaryPred,std::greater<>>::value || std::is_same<BinaryPred,std::less<>>::value)
		if(lhs.empty())
			return false;

	for(auto i = 0u; i < lhs.size(); ++i)
		if(!pred(lhs(i), rhs(i)))
			return false;
	return true;
}

template<class T, class F, class A, class UnaryPred>
bool compare(tensor<T,F,A> const& rhs, UnaryPred pred)
{
	for(auto i = 0u; i < rhs.size(); ++i)
		if(!pred(rhs(i)))
			return false;
	return true;
}

template<class T, class L, class R, class BinaryPred>
bool compare(tensor_expression<T,L> const& lhs, tensor_expression<T,R> const& rhs, BinaryPred pred)
{
	constexpr bool lhs_is_tensor = std::is_same<T,L>::value;
	constexpr bool rhs_is_tensor = std::is_same<T,R>::value;

	if constexpr (lhs_is_tensor && rhs_is_tensor)
		return compare(static_cast<T const&>( lhs ), static_cast<T const&>( rhs ), pred);
	else if constexpr (lhs_is_tensor && !rhs_is_tensor)
		return compare(static_cast<T const&>( lhs ), T( rhs ), pred);
	else if constexpr (!lhs_is_tensor && rhs_is_tensor)
		return compare(T( lhs ), static_cast<T const&>( rhs ), pred);
	else
		return compare(T( lhs ), T( rhs ), pred);

}

template<class T, class D, class UnaryPred>
bool compare(tensor_expression<T,D> const& expr, UnaryPred pred)
{
	if constexpr (std::is_same<T,D>::value)
		return compare(static_cast<T const&>( expr ), pred);
	else
		return compare(T( expr ), pred);
}

}

template<class T, class L, class R>
bool operator==( boost::numeric::ublas::detail::tensor_expression<T,L> const& lhs,
								 boost::numeric::ublas::detail::tensor_expression<T,R> const& rhs) {
	return boost::numeric::ublas::detail::compare( lhs, rhs, std::equal_to<>{} );
}
template<class T, class L, class R>
auto operator!=(boost::numeric::ublas::detail::tensor_expression<T,L> const& lhs,
								boost::numeric::ublas::detail::tensor_expression<T,R> const& rhs) {
	return boost::numeric::ublas::detail::compare( lhs, rhs, std::not_equal_to<>{}  );
}
template<class T, class L, class R>
auto operator< ( boost::numeric::ublas::detail::tensor_expression<T,L> const& lhs,
								 boost::numeric::ublas::detail::tensor_expression<T,R> const& rhs) {
	return boost::numeric::ublas::detail::compare( lhs, rhs, std::less<>{} );
}
template<class T, class L, class R>
auto operator<=( boost::numeric::ublas::detail::tensor_expression<T,L> const& lhs,
								 boost::numeric::ublas::detail::tensor_expression<T,R> const& rhs) {
	return boost::numeric::ublas::detail::compare( lhs, rhs, std::less_equal<>{} );
}
template<class T, class L, class R>
auto operator> ( boost::numeric::ublas::detail::tensor_expression<T,L> const& lhs,
								 boost::numeric::ublas::detail::tensor_expression<T,R> const& rhs) {
	return boost::numeric::ublas::detail::compare( lhs, rhs, std::greater<>{} );
}
template<class T, class L, class R>
auto operator>=( boost::numeric::ublas::detail::tensor_expression<T,L> const& lhs,
								 boost::numeric::ublas::detail::tensor_expression<T,R> const& rhs) {
	return boost::numeric::ublas::detail::compare( lhs, rhs, std::greater_equal<>{} );
}

template<class T, class D>
bool operator==( typename T::const_reference lhs, boost::numeric::ublas::detail::tensor_expression<T,D> const& rhs) {
	return boost::numeric::ublas::detail::compare( rhs, [lhs](auto const& r){ return lhs == r; } );
}
template<class T, class D>
auto operator!=( typename T::const_reference lhs, boost::numeric::ublas::detail::tensor_expression<T,D> const& rhs) {
	return boost::numeric::ublas::detail::compare( rhs, [lhs](auto const& r){ return lhs != r; } );
}
template<class T, class D>
auto operator< ( typename T::const_reference lhs, boost::numeric::ublas::detail::tensor_expression<T,D> const& rhs) {
	return boost::numeric::ublas::detail::compare( rhs, [lhs](auto const& r){ return lhs <  r; } );
}
template<class T, class D>
auto operator<=( typename T::const_reference lhs, boost::numeric::ublas::detail::tensor_expression<T,D> const& rhs) {
	return boost::numeric::ublas::detail::compare( rhs, [lhs](auto const& r){ return lhs <= r; } );
}
template<class T, class D>
auto operator> ( typename T::const_reference lhs, boost::numeric::ublas::detail::tensor_expression<T,D> const& rhs) {
	return boost::numeric::ublas::detail::compare( rhs, [lhs](auto const& r){ return lhs >  r; } );
}
template<class T, class D>
auto operator>=( typename T::const_reference lhs, boost::numeric::ublas::detail::tensor_expression<T,D> const& rhs) {
	return boost::numeric::ublas::detail::compare( rhs, [lhs](auto const& r){ return lhs >= r; } );
}

template<class T, class D>
bool operator==( boost::numeric::ublas::detail::tensor_expression<T,D> const& lhs, typename T::const_reference rhs) {
	return boost::numeric::ublas::detail::compare( lhs, [rhs](auto const& l){ return l == rhs; } );
}
template<class T, class D>
auto operator!=( boost::numeric::ublas::detail::tensor_expression<T,D> const& lhs, typename T::const_reference rhs) {
	return boost::numeric::ublas::detail::compare( lhs, [rhs](auto const& l){ return l != rhs; } );
}
template<class T, class D>
auto operator< ( boost::numeric::ublas::detail::tensor_expression<T,D> const& lhs, typename T::const_reference rhs) {
	return boost::numeric::ublas::detail::compare( lhs, [rhs](auto const& l){ return l <  rhs; } );
}
template<class T, class D>
auto operator<=( boost::numeric::ublas::detail::tensor_expression<T,D> const& lhs, typename T::const_reference rhs) {
	return boost::numeric::ublas::detail::compare( lhs, [rhs](auto const& l){ return l <= rhs; } );
}
template<class T, class D>
auto operator> ( boost::numeric::ublas::detail::tensor_expression<T,D> const& lhs, typename T::const_reference rhs) {
	return boost::numeric::ublas::detail::compare( lhs, [rhs](auto const& l){ return l >  rhs; } );
}
template<class T, class D>
auto operator>=( boost::numeric::ublas::detail::tensor_expression<T,D> const& lhs, typename T::const_reference rhs) {
	return boost::numeric::ublas::detail::compare( lhs, [rhs](auto const& l){ return l >= rhs; } );
}

#endif


//           Copyright Matthew Pulver 2018 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE_1_0.txt or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

// Contributors:
//  * Kedar R. Bhat - C++11 compatibility.

// Notes:
//  * Any changes to this file should always be downstream from autodiff.cpp.
//    C++17 is a higher-level language and is easier to maintain. For example, a number of functions which are
//    lucidly read in autodiff.cpp are forced to be split into multiple structs/functions in this file for
//    C++11.
//  * Use of typename RootType and SizeType is a hack to prevent Visual Studio 2015 from compiling functions
//    that are never called, that would otherwise produce compiler errors. Also forces functions to be inline.

#ifndef BOOST_MATH_DIFFERENTIATION_AUTODIFF_HPP
#error \
    "Do not #include this file directly. This should only be #included by autodiff.hpp for C++11 compatibility."
#endif

#include <type_traits>
#include <boost/math/tools/mp.hpp>

namespace boost {
namespace math {

namespace mp = tools::meta_programming;

namespace differentiation {
inline namespace autodiff_v1 {
namespace detail {

template <typename RealType, size_t Order>
fvar<RealType, Order>::fvar(root_type const& ca, bool const is_variable) {
  fvar_cpp11(is_fvar<RealType>{}, ca, is_variable);
}

template <typename RealType, size_t Order>
template <typename RootType>
void fvar<RealType, Order>::fvar_cpp11(std::true_type, RootType const& ca, bool const is_variable) {
  v.front() = RealType(ca, is_variable);
  if (0 < Order)
    std::fill(v.begin() + 1, v.end(), static_cast<RealType>(0));
}

template <typename RealType, size_t Order>
template <typename RootType>
void fvar<RealType, Order>::fvar_cpp11(std::false_type, RootType const& ca, bool const is_variable) {
  v.front() = ca;
  if (0 < Order) {
    v[1] = static_cast<root_type>(static_cast<int>(is_variable));
    if (1 < Order)
      std::fill(v.begin() + 2, v.end(), static_cast<RealType>(0));
  }
}

template <typename RealType, size_t Order>
template <typename... Orders>
get_type_at<RealType, sizeof...(Orders)> fvar<RealType, Order>::at_cpp11(std::true_type,
                                                                         size_t order,
                                                                         Orders...) const {
  return v.at(order);
}

template <typename RealType, size_t Order>
template <typename... Orders>
get_type_at<RealType, sizeof...(Orders)> fvar<RealType, Order>::at_cpp11(std::false_type,
                                                                         size_t order,
                                                                         Orders... orders) const {
  return v.at(order).at(orders...);
}

// Can throw "std::out_of_range: array::at: __n (which is 7) >= _Nm (which is 7)"
template <typename RealType, size_t Order>
template <typename... Orders>
get_type_at<RealType, sizeof...(Orders)> fvar<RealType, Order>::at(size_t order, Orders... orders) const {
  return at_cpp11(std::integral_constant<bool, sizeof...(orders) == 0>{}, order, orders...);
}

template <typename T, typename... Ts>
constexpr T product(Ts...) {
  return static_cast<T>(1);
}

template <typename T, typename... Ts>
constexpr T product(T factor, Ts... factors) {
  return factor * product<T>(factors...);
}

// Can throw "std::out_of_range: array::at: __n (which is 7) >= _Nm (which is 7)"
template <typename RealType, size_t Order>
template <typename... Orders>
get_type_at<fvar<RealType, Order>, sizeof...(Orders)> fvar<RealType, Order>::derivative(
    Orders... orders) const {
  static_assert(sizeof...(Orders) <= depth,
                "Number of parameters to derivative(...) cannot exceed fvar::depth.");
  return at(static_cast<size_t>(orders)...) *
         product(boost::math::factorial<root_type>(static_cast<unsigned>(orders))...);
}

template <typename RootType, typename Func>
class Curry {
  Func const& f_;
  size_t const i_;

 public:
  template <typename SizeType>  // typename SizeType to force inline constructor.
  Curry(Func const& f, SizeType i) : f_(f), i_(static_cast<std::size_t>(i)) {}
  template <typename... Indices>
  RootType operator()(Indices... indices) const {
    using unsigned_t = typename std::make_unsigned<typename std::common_type<Indices>::type...>::type;
    return f_(i_, static_cast<unsigned_t>(indices)...);
  }
};

template <typename RealType, size_t Order>
template <typename Func, typename Fvar, typename... Fvars>
promote<fvar<RealType, Order>, Fvar, Fvars...> fvar<RealType, Order>::apply_coefficients(
    size_t const order,
    Func const& f,
    Fvar const& cr,
    Fvars&&... fvars) const {
  fvar<RealType, Order> const epsilon = fvar<RealType, Order>(*this).set_root(0);
  size_t i = order < order_sum ? order : order_sum;
  using return_type = promote<fvar<RealType, Order>, Fvar, Fvars...>;
  return_type accumulator = cr.apply_coefficients(
      order - i, Curry<typename return_type::root_type, Func>(f, i), std::forward<Fvars>(fvars)...);
  while (i--)
    (accumulator *= epsilon) += cr.apply_coefficients(
        order - i, Curry<typename return_type::root_type, Func>(f, i), std::forward<Fvars>(fvars)...);
  return accumulator;
}

template <typename RealType, size_t Order>
template <typename Func, typename Fvar, typename... Fvars>
promote<fvar<RealType, Order>, Fvar, Fvars...> fvar<RealType, Order>::apply_coefficients_nonhorner(
    size_t const order,
    Func const& f,
    Fvar const& cr,
    Fvars&&... fvars) const {
  fvar<RealType, Order> const epsilon = fvar<RealType, Order>(*this).set_root(0);
  fvar<RealType, Order> epsilon_i = fvar<RealType, Order>(1);  // epsilon to the power of i
  using return_type = promote<fvar<RealType, Order>, Fvar, Fvars...>;
  return_type accumulator = cr.apply_coefficients_nonhorner(
      order, Curry<typename return_type::root_type, Func>(f, 0), std::forward<Fvars>(fvars)...);
  size_t const i_max = order < order_sum ? order : order_sum;
  for (size_t i = 1; i <= i_max; ++i) {
    epsilon_i = epsilon_i.epsilon_multiply(i - 1, 0, epsilon, 1, 0);
    accumulator += epsilon_i.epsilon_multiply(
        i,
        0,
        cr.apply_coefficients_nonhorner(
            order - i, Curry<typename return_type::root_type, Func>(f, i), std::forward<Fvars>(fvars)...),
        0,
        0);
  }
  return accumulator;
}

template <typename RealType, size_t Order>
template <typename Func, typename Fvar, typename... Fvars>
promote<fvar<RealType, Order>, Fvar, Fvars...> fvar<RealType, Order>::apply_derivatives(
    size_t const order,
    Func const& f,
    Fvar const& cr,
    Fvars&&... fvars) const {
  fvar<RealType, Order> const epsilon = fvar<RealType, Order>(*this).set_root(0);
  size_t i = order < order_sum ? order : order_sum;
  using return_type = promote<fvar<RealType, Order>, Fvar, Fvars...>;
  return_type accumulator =
      cr.apply_derivatives(
          order - i, Curry<typename return_type::root_type, Func>(f, i), std::forward<Fvars>(fvars)...) /
      factorial<root_type>(static_cast<unsigned>(i));
  while (i--)
    (accumulator *= epsilon) +=
        cr.apply_derivatives(
            order - i, Curry<typename return_type::root_type, Func>(f, i), std::forward<Fvars>(fvars)...) /
        factorial<root_type>(static_cast<unsigned>(i));
  return accumulator;
}

template <typename RealType, size_t Order>
template <typename Func, typename Fvar, typename... Fvars>
promote<fvar<RealType, Order>, Fvar, Fvars...> fvar<RealType, Order>::apply_derivatives_nonhorner(
    size_t const order,
    Func const& f,
    Fvar const& cr,
    Fvars&&... fvars) const {
  fvar<RealType, Order> const epsilon = fvar<RealType, Order>(*this).set_root(0);
  fvar<RealType, Order> epsilon_i = fvar<RealType, Order>(1);  // epsilon to the power of i
  using return_type = promote<fvar<RealType, Order>, Fvar, Fvars...>;
  return_type accumulator = cr.apply_derivatives_nonhorner(
      order, Curry<typename return_type::root_type, Func>(f, 0), std::forward<Fvars>(fvars)...);
  size_t const i_max = order < order_sum ? order : order_sum;
  for (size_t i = 1; i <= i_max; ++i) {
    epsilon_i = epsilon_i.epsilon_multiply(i - 1, 0, epsilon, 1, 0);
    accumulator += epsilon_i.epsilon_multiply(
        i,
        0,
        cr.apply_derivatives_nonhorner(
            order - i, Curry<typename return_type::root_type, Func>(f, i), std::forward<Fvars>(fvars)...) /
            factorial<root_type>(static_cast<unsigned>(i)),
        0,
        0);
  }
  return accumulator;
}

template <typename RealType, size_t Order>
template <typename SizeType>
fvar<RealType, Order> fvar<RealType, Order>::epsilon_multiply_cpp11(std::true_type,
                                                                    SizeType z0,
                                                                    size_t isum0,
                                                                    fvar<RealType, Order> const& cr,
                                                                    size_t z1,
                                                                    size_t isum1) const {
  size_t const m0 = order_sum + isum0 < Order + z0 ? Order + z0 - (order_sum + isum0) : 0;
  size_t const m1 = order_sum + isum1 < Order + z1 ? Order + z1 - (order_sum + isum1) : 0;
  size_t const i_max = m0 + m1 < Order ? Order - (m0 + m1) : 0;
  fvar<RealType, Order> retval = fvar<RealType, Order>();
  for (size_t i = 0, j = Order; i <= i_max; ++i, --j)
    retval.v[j] = epsilon_inner_product(z0, isum0, m0, cr, z1, isum1, m1, j);
  return retval;
}

template <typename RealType, size_t Order>
template <typename SizeType>
fvar<RealType, Order> fvar<RealType, Order>::epsilon_multiply_cpp11(std::false_type,
                                                                    SizeType z0,
                                                                    size_t isum0,
                                                                    fvar<RealType, Order> const& cr,
                                                                    size_t z1,
                                                                    size_t isum1) const {
  using ssize_t = typename std::make_signed<std::size_t>::type;
  RealType const zero(0);
  size_t const m0 = order_sum + isum0 < Order + z0 ? Order + z0 - (order_sum + isum0) : 0;
  size_t const m1 = order_sum + isum1 < Order + z1 ? Order + z1 - (order_sum + isum1) : 0;
  size_t const i_max = m0 + m1 < Order ? Order - (m0 + m1) : 0;
  fvar<RealType, Order> retval = fvar<RealType, Order>();
  for (size_t i = 0, j = Order; i <= i_max; ++i, --j)
    retval.v[j] = std::inner_product(
        v.cbegin() + ssize_t(m0), v.cend() - ssize_t(i + m1), cr.v.crbegin() + ssize_t(i + m0), zero);
  return retval;
}

template <typename RealType, size_t Order>
fvar<RealType, Order> fvar<RealType, Order>::epsilon_multiply(size_t z0,
                                                              size_t isum0,
                                                              fvar<RealType, Order> const& cr,
                                                              size_t z1,
                                                              size_t isum1) const {
  return epsilon_multiply_cpp11(is_fvar<RealType>{}, z0, isum0, cr, z1, isum1);
}

template <typename RealType, size_t Order>
template <typename SizeType>
fvar<RealType, Order> fvar<RealType, Order>::epsilon_multiply_cpp11(std::true_type,
                                                                    SizeType z0,
                                                                    size_t isum0,
                                                                    root_type const& ca) const {
  fvar<RealType, Order> retval(*this);
  size_t const m0 = order_sum + isum0 < Order + z0 ? Order + z0 - (order_sum + isum0) : 0;
  for (size_t i = m0; i <= Order; ++i)
    retval.v[i] = retval.v[i].epsilon_multiply(z0, isum0 + i, ca);
  return retval;
}

template <typename RealType, size_t Order>
template <typename SizeType>
fvar<RealType, Order> fvar<RealType, Order>::epsilon_multiply_cpp11(std::false_type,
                                                                    SizeType z0,
                                                                    size_t isum0,
                                                                    root_type const& ca) const {
  fvar<RealType, Order> retval(*this);
  size_t const m0 = order_sum + isum0 < Order + z0 ? Order + z0 - (order_sum + isum0) : 0;
  for (size_t i = m0; i <= Order; ++i)
    if (retval.v[i] != static_cast<RealType>(0))
      retval.v[i] *= ca;
  return retval;
}

template <typename RealType, size_t Order>
fvar<RealType, Order> fvar<RealType, Order>::epsilon_multiply(size_t z0,
                                                              size_t isum0,
                                                              root_type const& ca) const {
  return epsilon_multiply_cpp11(is_fvar<RealType>{}, z0, isum0, ca);
}

template <typename RealType, size_t Order>
template <typename RootType>
fvar<RealType, Order>& fvar<RealType, Order>::multiply_assign_by_root_type_cpp11(std::true_type,
                                                                                bool is_root,
                                                                                RootType const& ca) {
  auto itr = v.begin();
  itr->multiply_assign_by_root_type(is_root, ca);
  for (++itr; itr != v.end(); ++itr)
    itr->multiply_assign_by_root_type(false, ca);
  return *this;
}

template <typename RealType, size_t Order>
template <typename RootType>
fvar<RealType, Order>& fvar<RealType, Order>::multiply_assign_by_root_type_cpp11(std::false_type,
                                                                                bool is_root,
                                                                                RootType const& ca) {
  auto itr = v.begin();
  if (is_root || *itr != 0)
    *itr *= ca;  // Skip multiplication of 0 by ca=inf to avoid nan, except when is_root.
  for (++itr; itr != v.end(); ++itr)
    if (*itr != 0)
      *itr *= ca;
  return *this;
}

template <typename RealType, size_t Order>
fvar<RealType, Order>& fvar<RealType, Order>::multiply_assign_by_root_type(bool is_root,
                                                                           root_type const& ca) {
  return multiply_assign_by_root_type_cpp11(is_fvar<RealType>{}, is_root, ca);
}

template <typename RealType, size_t Order>
template <typename RootType>
fvar<RealType, Order>& fvar<RealType, Order>::negate_cpp11(std::true_type, RootType const&) {
  std::for_each(v.begin(), v.end(), [](RealType& r) { r.negate(); });
  return *this;
}

template <typename RealType, size_t Order>
template <typename RootType>
fvar<RealType, Order>& fvar<RealType, Order>::negate_cpp11(std::false_type, RootType const&) {
  std::for_each(v.begin(), v.end(), [](RealType& a) { a = -a; });
  return *this;
}

template <typename RealType, size_t Order>
fvar<RealType, Order>& fvar<RealType, Order>::negate() {
  return negate_cpp11(is_fvar<RealType>{}, static_cast<root_type>(*this));
}

template <typename RealType, size_t Order>
template <typename RootType>
fvar<RealType, Order>& fvar<RealType, Order>::set_root_cpp11(std::true_type, RootType const& root) {
  v.front().set_root(root);
  return *this;
}

template <typename RealType, size_t Order>
template <typename RootType>
fvar<RealType, Order>& fvar<RealType, Order>::set_root_cpp11(std::false_type, RootType const& root) {
  v.front() = root;
  return *this;
}

template <typename RealType, size_t Order>
fvar<RealType, Order>& fvar<RealType, Order>::set_root(root_type const& root) {
  return set_root_cpp11(is_fvar<RealType>{}, root);
}

template <typename RealType, size_t Order, size_t... Is>
auto make_fvar_for_tuple(mp::index_sequence<Is...>, RealType const& ca)
    -> decltype(make_fvar<RealType, zero<Is>::value..., Order>(ca)) {
  return make_fvar<RealType, zero<Is>::value..., Order>(ca);
}

template <typename RealType, size_t... Orders, size_t... Is, typename... RealTypes>
auto make_ftuple_impl(mp::index_sequence<Is...>, RealTypes const&... ca)
    -> decltype(std::make_tuple(make_fvar_for_tuple<RealType, Orders>(mp::make_index_sequence<Is>{},
                                                                      ca)...)) {
  return std::make_tuple(make_fvar_for_tuple<RealType, Orders>(mp::make_index_sequence<Is>{}, ca)...);
}

}  // namespace detail

template <typename RealType, size_t... Orders, typename... RealTypes>
auto make_ftuple(RealTypes const&... ca)
    -> decltype(detail::make_ftuple_impl<RealType, Orders...>(mp::index_sequence_for<RealTypes...>{},
                                                              ca...)) {
  static_assert(sizeof...(Orders) == sizeof...(RealTypes),
                "Number of Orders must match number of function parameters.");
  return detail::make_ftuple_impl<RealType, Orders...>(mp::index_sequence_for<RealTypes...>{}, ca...);
}

}  // namespace autodiff_v1
}  // namespace differentiation
}  // namespace math
}  // namespace boost


#pragma once
/**
 * @file pauli_operator.hpp
 * @brief Definition and basic functions for MultiPauliTerm
 */

#pragma once

#include <boost/dynamic_bitset.hpp>
#include <cassert>
#include <iostream>
#include <vector>

#include "exception.hpp"
#include "type.hpp"

class QuantumStateBase;

/**
 * \~japanese-en
 * @struct SiglePauliOperator
 * 単一qubitに作用するパウリ演算子の情報を保持するクラス。
 * 作用するqubitの添字と自身のパウリ演算子の情報をもつ。
 */
class DllExport SinglePauliOperator {
protected:
    UINT _index;
    UINT _pauli_id;

public:
    /**
     * \~japanese-en
     * コンストラクタ
     *
     * 添字とパウリ演算子からインスタンスを生成する。
     *
     * @param[in] index_ 作用するqubitの添字
     * @param[in] pauli_id_
     * パウリ演算子を表す整数。(I,X,Y,Z)が(0,1,2,3)に対応する。
     * @return 新しいインスタンス
     */
    SinglePauliOperator(UINT index_, UINT pauli_id_)
        : _index(index_), _pauli_id(pauli_id_) {
        if (pauli_id_ > 3) {
            throw InvalidPauliIdentifierException(
                "Error: SinglePauliOperator(UINT, UINT): index must be "
                "either of 0,1,2,3");
        }
    };

    /**
     * \~japanese-en
     * 自身が作用する添字を返す
     *
     * @return 自身が作用する添字
     */
    UINT index() const { return _index; }

    /**
     * \~japanese-en
     * 自身を表すパウリ演算子を返す
     *
     * @return
     * 自身のもつパウリ演算子を表す整数。(I,X,Y,Z)が(0,1,2,3)に対応する。
     */
    UINT pauli_id() const { return _pauli_id; }
};

/**
 * \~japanese-en
 * @struct PauliOperator
 * 複数qubitに作用するパウリ演算子の情報を保持するクラス。
 * SinglePauliOperatorをリストとして持ち, 種々の操作を行う。
 */
class DllExport PauliOperator {
private:
    std::vector<SinglePauliOperator> _pauli_list;
    CPPCTYPE _coef;
    boost::dynamic_bitset<> _z;
    boost::dynamic_bitset<> _x;

public:
    /**
     * \~japanese-en
     * 自身の保持するパウリ演算子が作用する添字のリストを返す
     *
     * それぞれの添字に作用する演算子は
     * PauliOperator::get_pauli_id_listで得られる添字のリストの対応する場所から得られる。
     *
     * @return 自身の保持するパウリ演算子が作用する添字のリスト。
     */
    std::vector<UINT> get_index_list() const {
        std::vector<UINT> index_list;
        std::transform(_pauli_list.cbegin(), _pauli_list.cend(),
            std::back_inserter(index_list),
            [](auto val) { return val.index(); });
        return index_list;
    }

    /**
     * \~japanese-en
     * 自身の保持するパウリ演算子が添え字のうち、最大の添え字を返す
     *
     * @return 自身の保持するパウリ演算子が作用する添字のうち最大の整数
     */
    UINT get_qubit_count() const {
        std::vector<UINT> index_list = get_index_list();
        if (index_list.size() == 0) return 0;
        return *std::max_element(index_list.begin(), index_list.end()) + 1;
    }

    /**
     * \~japanese-en
     * 自身が保持するパウリ演算子を返す。
     *
     * それぞれが作用するqubitは
     * PauliOperator::get_index_listで得られる添字のリストの対応する場所から得られる。
     *
     * @return
     * 自身の保持するパウリ演算子のリスト。(I,X,Y,Z)が(0,1,2,3)に対応する。
     */
    std::vector<UINT> get_pauli_id_list() const {
        std::vector<UINT> pauli_id_list;
        std::transform(_pauli_list.cbegin(), _pauli_list.cend(),
            std::back_inserter(pauli_id_list),
            [](auto val) { return val.pauli_id(); });
        return pauli_id_list;
    }

    /**
     * \~japanese-en
     * コンストラクタ
     *
     * 係数をとって空のインスタンスを返す。
     *
     * @param[in] coef 係数。デフォルトは1.0
     * @return 係数がcoefの空のインスタンス
     */
    explicit PauliOperator(CPPCTYPE coef = 1.) : _coef(coef){};

    /**
     * \~japanese-en
     * コンストラクタ
     *
     * パウリ演算子とその添字からなる文字列と、その係数から複数qubitに掛かるパウリ演算子を作成する
     *
     * @param[in] strings Pauli演算子とその掛かるindex. "X 1 Y 2 Z
     * 5"のようにスペース区切りの文字列
     * @param[in] coef 演算子の係数
     * @return 入力のパウリ演算子と係数をもつPauliOpetatorのインスタンス
     */
    explicit PauliOperator(std::string strings, CPPCTYPE coef = 1.);

    /**
     * \~japanese-en
     * コンストラクタ
     *
     * パウリ演算子の文字列と添字のリスト、係数からPauliOperatorのインスタンスを生成する。
     * このとき入力として与える演算子と添字のリストは、i番目の演算子にi番目の添字が対応する。
     *
     * @param[in] target_qubit_index_list
     * Pauli_operator_type_listで与えるパウリ演算子が掛かるqubitを指定する添字のリスト。
     * @param[in] Pauli_operator_type_list パウリ演算子の文字列。(example:
     * "XXYZ")
     * @param[in] coef 係数
     * @return
     * 入力として与えたパウリ演算子のリストと添字のリスト、係数から生成されるPauliOperatorのインスタンス
     */
    PauliOperator(const std::vector<UINT>& target_qubit_index_list,
        std::string Pauli_operator_type_list, CPPCTYPE coef = 1.);

    /**
     * \~japanese-en
     * コンストラクタ
     *
     * 配列の添字に作用するパウリ演算子と係数からインスタンスを生成する。
     * @param[in] pauli_list
     * 配列の添字に対応するqubitに作用するパウリ演算子のリスト
     * @param[in] coef 係数
     * @return
     * pauli_listの添字に対応するqubitに作用するパウリ演算子と係数をもつインスタンス
     */
    explicit PauliOperator(
        const std::vector<UINT>& pauli_list, CPPCTYPE coef = 1.);

    /**
     * \~japanese-en
     * コンストラクタ
     *
     * パウリ演算子のリストと添字のリスト、係数からPauliOperatorのインスタンスを生成する。
     * このとき入力として与える演算子と添字のリストは、リストの同じ添字の場所にあるものが対応する。
     *
     * @param[in] target_qubit_index_list
     * Pauli_operator_type_listで与えるパウリ演算子が掛かるqubitを指定する添字のリスト
     * @param[in] target_qubit_pauli_list
     * パウリ演算子の符号なし整数リスト。(I,X,Y,Z)が(0,1,2,3)に対応する。
     * @param[in] coef 係数
     * @return
     * 入力として与えたパウリ演算子のリストと添字のリスト、係数から生成されるPauliOperatorのインスタンス
     */
    PauliOperator(const std::vector<UINT>& target_qubit_index_list,
        const std::vector<UINT>& target_qubit_pauli_list, CPPCTYPE coef = 1.);

    PauliOperator(const boost::dynamic_bitset<>& x,
        const boost::dynamic_bitset<>& z, CPPCTYPE coef = 1.);

    /**
     * \~japanese-en
     * 自身の係数を返す
     *
     * @return 自身の係数
     */
    virtual CPPCTYPE get_coef() const { return _coef; }

    /**
     * \~japanese-en
     * 自身のxビットを返す
     *
     * @return 自身のxビット
     */
    virtual boost::dynamic_bitset<> get_x_bits() const { return _x; }

    /**
     * \~japanese-en
     * 自身のzビットを返す
     *
     * @return 自身のzビット
     */
    virtual boost::dynamic_bitset<> get_z_bits() const { return _z; }

    virtual ~PauliOperator(){};

    /**
     * \~japanese-en
     * 指定した添字のqubitに作用するSinglePauliOperatorを自身が保持するリストの末尾に追加する。
     *
     * @param[in] qubit_index 作用するqubitの添字
     * @param[in] pauli_type パウリ演算子。(I,X,Y,Z)が(0,1,2,3)に対応する。
     */
    virtual void add_single_Pauli(UINT qubit_index, UINT pauli_type);

    /**
     * \~japanese-en
     * 量子状態に対応するパウリ演算子の期待値を計算する
     *
     * @param[in] state 期待値をとるときの量子状態
     * @return stateに対応する期待値
     */
    virtual CPPCTYPE get_expectation_value(const QuantumStateBase* state) const;

    /**
     * \~japanese-en
     * added by myself
     * 量子状態に対応するパウリ演算子の期待値を計算する
     * get_expectation_value の 1 スレッドバージョン
     *
     * @param[in] state 期待値をとるときの量子状態
     * @return stateに対応する期待値
     */
    virtual CPPCTYPE get_expectation_value_single_thread(
        const QuantumStateBase* state) const;

    /**
     * \~japanese-en
     * 量子状態に対応するパウリ演算子の遷移振幅を計算する
     *
     * @param[in] state_bra 遷移先の量子状態
     * @param[in] state_ket 遷移元の量子状態
     * @return state_bra, state_ketに対応する遷移振幅
     */
    virtual CPPCTYPE get_transition_amplitude(const QuantumStateBase* state_bra,
        const QuantumStateBase* state_ket) const;

    /**
     * \~japanese-en
     * 自身のディープコピーを生成する
     *
     * @return 自身のディープコピー
     */
    virtual PauliOperator* copy() const;

    virtual void change_coef(CPPCTYPE new_coef);

    /**
     * \~japanese-en
     * パウリ演算子に対応する文字列を返す
     */
    virtual std::string get_pauli_string() const;
    /**
     * \~japanese-en
     * このオブザーバブルに入っているものを、ゲートとしてstateに作用させる
     * @param [in] state 入力
     */
    virtual void update_quantum_state(QuantumStateBase* instate);

    PauliOperator operator*(const PauliOperator& target) const;

    PauliOperator operator*(CPPCTYPE target) const;

    PauliOperator& operator*=(const PauliOperator& target);

    PauliOperator& operator*=(CPPCTYPE target);
};


/*
  Copyright (C) 2016 Ahmed Riza

  This file is part of MathFin.

  This program is free software: you  can redistribute it and/or modify it
  under the  terms of the GNU  General Public License as  published by the
  Free Software Foundation,  either version 3 of the License,  or (at your
  option) any later version.

  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 GNU General
  Public License for more details.

  You should have received a copy  of the GNU General Public License along
  with this program. If not, see <http://www.gnu.org/licenses/>.
*/

/*
  Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
  Copyright (C) 2003, 2004, 2005, 2006 StatPro Italia srl
  Copyright (C) 2004, 2005, 2006 Ferdinando Ametrano
  Copyright (C) 2006 Katiuscia Manzoni
  Copyright (C) 2006 Toyin Akin
  Copyright (C) 2015 Klaus Spanderen

  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.
*/

#ifndef MATHFIN_DATE_HPP
#define MATHFIN_DATE_HPP

#include <time/period.hpp>
#include <time/month.hpp>
#include <time/weekday.hpp>
#include <boost/date_time/posix_time/ptime.hpp>
#include <boost/date_time/posix_time/posix_time_duration.hpp>

namespace MathFin {

  /**
   * Day number
   * @ingroup datetime
   */
  typedef Integer Day;

  /**
   * Year number
   * @ingroup datetime
   */
  typedef Integer Year;

  /**
   * Hour number
   * @ingroup datetime
   */
  typedef boost::posix_time::hours::hour_type Hour;

  /**
   * Minute number
   * @ingroup datetime
   */
  typedef boost::posix_time::minutes::min_type Minute;

  /**
   * Second number
   * @ingroup datetime
   */
  typedef boost::posix_time::minutes::sec_type Second;

  /**
   * Millisecond number
   * @ingroup datetime
   */
  typedef boost::posix_time::time_duration::fractional_seconds_type Millisecond;

  /**
   * Microsecond number
   * @ingroup datetime
   */
  typedef boost::posix_time::time_duration::fractional_seconds_type Microsecond;

  /**
   * Date class.
   *
   * This class provides methods to inspect dates as well as methods and
   * operators which implement a limited date algebra (increasing and
   * decreasing dates, and calculating their difference).
   *
   * The maximal resolution of the methods is either micro or nano seconds
   * depending on the underlying boost installation.
   *
   * @ingroup datetime
   */
  class Date {
  public:
    typedef boost::int_fast32_t serial_type;

    /**
     * @name constructors
     * @{
     */

    /**
     * Default constructor returning a null date.
     */
    Date();

    /**
     * Constructor taking a serial number as given by Excel.
     */
    explicit Date(Date::serial_type serialNumber);

    /**
     * Constructor taking day, month and year.
     */
    explicit Date(Day d, Month m, Year y);

    /**
     * Constructor taking boost posix date time object
     */
    explicit Date(const boost::posix_time::ptime& localTime);

    /**
     * Constructor taking the full range of parameters.
     */
    explicit Date(Day d,
         Month m,
         Year y,
         Hour hours,
         Minute minutes,
         Second seconds,
         Millisecond millisec = 0,
         Microsecond microsec = 0);

    /** @} */ // end of constructors.

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

    /**
     * @name inspectors
     * @{
     */

    /**
     * Get week day.
     */
    Weekday weekday() const;

    /**
     * Get day of month.
     */
    Day dayOfMonth() const;

    /**
     * Get day of the year; one-based (Jan 1st = 1)
     */
    Day dayOfYear() const;

    /**
     *  Get month
     */
    Month month() const;

    /**
     * Get year.
     */
    Year year() const;

    /**
     * Get serial number.
     */
    Date::serial_type serialNumber() const;

    /**
     * Get hours.
     */
    Hour hours() const;

    /**
     * Get minutes.
     */
    Minute minutes() const;

    /**
     * Get seconds.
     */
    Second seconds() const;

    /**
     * Get  millseconds.
     */
    Millisecond milliseconds() const;

    /**
     *  Get microseconds.
     */
    Microsecond microseconds() const;

    /**
     * Get fraction of day represent by this Date instance.
     * The maximal resolution of the methods is either micro or nano seconds
     * depending on the underlying boost installation.
     */
    Time fractionOfDay() const;

    /**
     * Get fraction of second represented by this Date instance.
     * The maximal resolution of the methods is either micro or nano seconds
     * depending on the underlying boost installation.
     */
    Time fractionOfSecond() const;

    const boost::posix_time::ptime& dateTime() const { return dateTime_; }

    /**
     * Get the number of days in the year.
     * If the year is a leap year, return 366 otherwise return 365.
     */
    Real lengthOfYear() const;

    /** @} */ // end of inspectors.

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

    /**
     * @name date algebra
     * @{
     */

    /**
     * returns a new date incremented by the given number of days
     */
    Date operator+(Date::serial_type days) const;

    /**
     * returns a new date incremented by the given period
     */
    Date operator+(const Period&) const;

    /**
     * returns a new date decremented by the given number of days
     */
    Date operator-(Date::serial_type days) const;

    /**
     * returns a new date decremented by the given period
     */
    Date operator-(const Period&) const;

    /** @} */ // end of date algebra methods.

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

    /**
     * today's date.
     */
    static Date todaysDate();

    /**
     * earliest allowed date
     */
    static Date minDate();

    /**
     * latest allowed date
     */
    static Date maxDate();

    /**
     * whether the given year is a leap one
     */
    static bool isLeap(Year y);

    /**
     * last day of the month to which the given date belongs
     */
    static Date endOfMonth(const Date& d);

    /**
     * whether a date is the last day of its month
     */
    static bool isEndOfMonth(const Date& d);

    /**
     * next given weekday following or equal to the given date
     * E.g., the Friday following Tuesday, January 15th, 2002
     * was January 18th, 2002.
     * @see http://www.cpearson.com/excel/DateTimeWS.htm
     */
    static Date nextWeekday(const Date& d, Weekday w);

    /**
     * n-th given weekday in the given month and year
     * E.g., the 4th Thursday of March, 1998 was March 26th,
     * 1998.
     * @see http://www.cpearson.com/excel/DateTimeWS.htm
     */
    static Date nthWeekday(Size n, Weekday w, Month m, Year y);

    /**
     * local date time, based on the time zone settings of the computer
     */
    static Date localDateTime();

    /**
     * UTC date time
     */
    static Date universalDateTime();

    /**
     *  underlying resolution of the  posix date time object
     */
    static const Size ticksPerSecond();

  private:
    Date& operator=(const Date&) = delete;

    static Date::serial_type minimumSerialNumber();
    static Date::serial_type maximumSerialNumber();
    static void checkSerialNumber(Date::serial_type serialNumber);

    const boost::posix_time::ptime dateTime_;
  };

  /**
   * Difference in days between dates.
   * @relates Date
   */
  Date::serial_type operator-(const Date&, const Date&);

  /**
   * Difference in days (including fraction of days) between dates
   * @relates Date
   */
  Time daysBetween(const Date& d1, const Date& d2);

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

  /**
   * Equivalence operator.
   * @relates Date
   */
  bool operator==(const Date&, const Date&);

  /**
   * Not equals operator.
   * @relates Date
   */
  bool operator!=(const Date&, const Date&);

  /**
   * Less than operator.
   * @relates Date
   */
  bool operator<(const Date&, const Date&);

  /**
   * Less than or equals operator.
   * @relates Date
   */
  bool operator<=(const Date&, const Date&);

  /**
   * Greater than operator.
   * @relates Date
   */
  bool operator>(const Date&, const Date&);

  /**
   * Greater than or equals operator.
   * @relates Date
   */
  bool operator>=(const Date&, const Date&);

  /**
   * Output operator.
   * @relates Date
   */
  std::ostream& operator<<(std::ostream&, const Date&);

}

#endif /* MATHFIN_DATE_HPP */


/* $Id: formula.hpp 48740 2011-03-05 10:01:34Z mordante $ */
/*
   Copyright (C) 2008 - 2011 by Mark de Wever <koraq@xs4all.nl>
   Part of the Battle for Wesnoth Project http://www.wesnoth.org/

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY.

   See the COPYING file for more details.
*/

#ifndef GUI_WIDGETS_FORMULA_HPP_INCLUDED
#define GUI_WIDGETS_FORMULA_HPP_INCLUDED

#include "formula_callable.hpp"
#include "../../formula.hpp"
#include "gui/auxiliary/log.hpp"
#include "gui/widgets/helper.hpp"
#include "serialization/string_utils.hpp"
#include "util.hpp"
#include "tstring.hpp"

#include <boost/static_assert.hpp>

#include <cassert>

namespace gui2{

/**
 * Template class can hold a value or a formula to calculate the value.
 *
 * A string is a formula when it starts with a right paren, no other validation
 * is done by this function, leading whitespace is significant.
 *
 * Upon getting the value of the formula a variable map is send. The variables
 * in the map can be used in the formula. The 'owners' of the class need to
 * document the variables available.
 *
 * @tparam T                      The type of the formula. This type needs to
 *                                be constructable form a string, either by a
 *                                lexical_cast or a template specialization in
 *                                this header.
 */
template <class T>
class tformula
{
public:
	/**
	 * Constructor.
	 *
	 * @param str                 The string used to initialize the class, this
	 *                            can either be a formula or a string which can
	 *                            be converted to the type T.
	 * @param value               The default value for the object.
	 */
	tformula<T>(const std::string& str, const T value = T());

	/**
	 * Returns the value, can only be used if the data is no formula.
	 *
	 * Another option would be to cache the output of the formula in value_
	 * and always allow this function. But for now decided that the caller
	 * needs to do the caching. It might be changed later.
	 */
	T operator()() const
	{
		assert(!has_formula());
		return value_;
	}

	/** Returns the value, can always be used. */
	T operator() (const game_logic::map_formula_callable& variables) const;

	/** Determine whether the class contains a formula. */
	bool has_formula() const { return !formula_.empty(); }

private:

	/**
	 * Converts the string to the template type.
	 *
	 * This function is used by the constructor to convert the string to the
	 * wanted value, if not a formula.
	 *
	 * @param str                 The str send to the constructor.
	 */
	void convert(const std::string& str);

	/**
	 * Executes the formula.
	 *
	 * This function does the calculation and can only be called if the object
	 * contains a formula.
	 *
	 * @param variables           The state variables which might be used in
	 *                            the formula. For example a screen_width can
	 *                            be set so the formula can return the half
	 *                            width of the screen.
	 *
	 * @returns                   The calculated value.
	 */
	T execute(const game_logic::map_formula_callable& variables) const;

	/**
	 * Contains the formuale for the variable.
	 *
	 * If the string is empty, there's no formula.
	 */
	std::string formula_;

	/** If there's no formula it contains the value. */
	T value_;
};

template<class T>
tformula<T>::tformula(const std::string& str, const T value) :
	formula_(),
	value_(value)
{
	if(str.empty()) {
		return;
	}

	if(str[0] == '(') {
		formula_ = str;
	} else {
		convert(str);
	}
}

template<class T>
inline T tformula<T>::operator()(
		const game_logic::map_formula_callable& variables) const
{
	if(has_formula()) {
		const T& result = execute(variables);
		LOG_GUI_D << "Formula: execute '" << formula_
			<< "' result '" << result
			<< "'.\n";
		return result;
	} else {
		return value_;
	}
}

template<>
inline bool tformula<bool>::execute(
		const game_logic::map_formula_callable& variables) const
{
	return game_logic::formula(formula_).evaluate(variables).as_bool();
}

template<>
inline int tformula<int>::execute(
		const game_logic::map_formula_callable& variables) const
{
	return game_logic::formula(formula_).evaluate(variables).as_int();
}

template<>
inline unsigned tformula<unsigned>::execute(
		const game_logic::map_formula_callable& variables) const
{
	return game_logic::formula(formula_).evaluate(variables).as_int();
}

template<>
inline std::string tformula<std::string>::execute(
		const game_logic::map_formula_callable& variables) const
{
	return game_logic::formula(formula_).evaluate(variables).as_string();
}

template<>
inline t_string tformula<t_string>::execute(
		const game_logic::map_formula_callable& variables) const
{
	return game_logic::formula(formula_).evaluate(variables).as_string();
}

template<>
inline PangoAlignment tformula<PangoAlignment>::execute(
		const game_logic::map_formula_callable& variables) const
{
	return decode_text_alignment(
			game_logic::formula(formula_).evaluate(variables).as_string());
}

template<class T>
inline T tformula<T>::execute(
		const game_logic::map_formula_callable& variables) const
{
	// Every type needs its own execute function avoid instantiation of the
	// default execute.
	BOOST_STATIC_ASSERT(sizeof(T) == 0);
	return T();
}

template<>
inline void tformula<bool>::convert(const std::string& str)
{
	value_ = utils::string_bool(str);
}

template<>
inline void tformula<std::string>::convert(const std::string& str)
{
	value_ = str;
}

template<>
inline void tformula<t_string>::convert(const std::string& str)
{
	value_ = str;
}

template<>
inline void tformula<PangoAlignment>::convert(const std::string& str)
{
	value_ = decode_text_alignment(str);
}

template<class T>
inline void tformula<T>::convert(const std::string& str)
{
	value_ = lexical_cast_default<T>(str);
}

} // namespace gui2

#endif


/******

六：成员函数模板
6.1 基本概念、构造函数模板
一些说法：
a)类模板中的成员函数，只有源程序代码中出现调用这些成员函数的代码时，
  这些成员函数才会出现在一个实例化了的类模板中。

b)类模板中的成员函数模板，只有源程序代码中出现调用这些成员函数模板的代码时，
  这些成员函数模板的具体实例才会出现在一个实例化了的类模板中。

c)目前编译器并不支持虚成员函数模板，因为虚函数表vtbl的大小是固定的。

C++之父的说法：如果允许虚函数模板，则每次有人用新的参数类型调用该虚函数模板时，就必须给对应的虚函数表再增加一项，
这意味着只有链接程序才能去构造虚函数表并在表中设置有关函数，因此，成员函数模板绝不能是虚的。
类模板中可以有普通的虚成员函数（虚函数），这并没有什么问题。大家都知道，普通成员函数如果不被调用的情况下不会被实例化出来。
但是，对于虚函数，不管是否调用，编译器都会把他实例化出来，因为编译器要创建虚函数表vtbl，该表中的每个具体表项都对应一个
虚函数地址，所以编译器必然得把所有虚函数都实例化出来。

6.2 拷贝构造函数模板与拷贝赋值运算符模板
请大家注意区分：拷贝构造函数模板不是拷贝构造函数，拷贝赋值运算符模板不是拷贝赋值运算符
（构造函数模板也不是构造函数）；
因为拷贝构造函数或者拷贝赋值运算符要求拷贝的对象类型完全相同，而拷贝构造函数模板和拷贝
赋值运算符模板就没有这种要求。

发现两个问题：
a)_nmsp1::A<float> a4(a3);代码行没输出任何结果，这表示拷贝构造函数模板中的代码没有执行。

b)a4.m_ic值的确变成了16.2了。这说明确实是通过a3拷贝构造生成的a4。
a3,a4类型相同（A<float>），本该执行拷贝构造函数，但是因为类模板A中没有拷贝构造函数，所以编译器内部实际是
执行了按值拷贝的一个动作，使a4.m_ic值变成了16.2了。
【拷贝构造函数模板永远不可能成为拷贝构造函数。编译器不会用调用拷贝构造函数模板来代替调用拷贝构造函数。】
拷贝构造函数模板什么时候被调用？类型不同（都是用类模板A实例化出来的，比如A<double>与A<float>）的两个对象，
用一个拷贝构造另一个时。

拷贝赋值运算符模板永远不可能成为拷贝赋值运算符

****/

#include <iostream>
//#include <boost/type_index.hpp>
using namespace std;
//#pragma warning(disable : 4996)

//6.1
namespace _nmsp1
{

	template <typename T1>
	class A
	{
	public:
		template <typename T2>
		A(T2 v1, T2 v2); //构造函数模板，引入了自己的模板参数T2，与类A的模板参数T1没有关系

	public:
		A(double v1, double v2)
		{
			cout << "A::A(double,double)执行了!" << endl;
		}
		A(T1 v1, T1 v2)
		{
			cout << "A::A(T1,T1)执行了!" << endl;
		}

	public:
		//拷贝构造函数模板
		template <typename U>
		A(const A<U>& other)
		//A(A<U>& other)
		{
			cout << "A::A(const A<U>& other)拷贝构造函数模板执行了!" << endl;
		}

		//拷贝赋值运算符模板
		template <typename U>
		A<T1>& operator=(const A<U>& other)
		//A<T1>& operator=(A<U>& other)
		{
			//.....
			cout << "operator=(const A<U>& other)拷贝赋值运算符模板执行了!" << endl;
			return *this;
		}

		////拷贝赋值运算符
		//A<T1>& operator=(const A<T1>& other)
		//{
		//	cout << "operator=(const A<T1>& other)拷贝赋值运算符执行了!" << endl;
		//	return *this;
		//}

		//---------------------------------
		/*template <typename T4>
		virtual void myvirfunc() {}*/
		virtual void myvirfunc2() {}

		//----------------------------------
		template <typename T3>
		void myft(T3 tmpt) //普通成员函数模板
		{
			cout << tmpt << endl;
		}

		T1 m_ic;
		static constexpr int m_stcvalue = 200;
	};

	//在类外实现类模板的构造函数模板
	template <typename T1>
	template <typename T2>
	A<T1>::A(T2 v1, T2 v2)
	{
		cout << "A::A(T2,T2)执行了!" << endl;
	}

}

//void f(int tm) {}
//void f2(int tm) {}

int main()
{
	_nmsp1::A<float> a(1, 2);           //实例化了A<float>这样一个类型，并用int类型来实例化构造函数
	a.myft(3);                          //3
	_nmsp1::A<float> a2(1.1, 2.2);      //A<float>类型已经被上面的代码实例化过了，并用doule类型来实例化构造函数,因为1.1和2.2都是double类型
	_nmsp1::A<float> a3(11.1f, 12.2f);  //用float类型来实例化构造函数,因为11.1f和12.2f都是float类型

	//----------------
	a3.m_ic = 16.2f;
	_nmsp1::A<float> a4(a3);   //会执行拷贝构造函数模板中的代码吗(不会)

	//----------------
	_nmsp1::A<int> a5(a3);     //a3是A<float>类型，a5是A<int>类型，两者类型不同

	//----------------
	a3 = a4;
	a3 = a5;

	return 0;
}


/*
Copyright (c) by respective owners including Yahoo!, Microsoft, and
individual contributors. All rights reserved.  Released under a BSD (revised)
license as described in the file LICENSE.
 */
#include <fstream>
#include <vector>
#include <float.h>
#ifdef _WIN32
#include <winsock2.h>
#else
#include <netdb.h>
#endif
#include <string.h>
#include <stdio.h>
#include <assert.h>
#include "constant.h"
#include "sparse_dense.h"
#include "gd.h"
#include "simple_label.h"
#include "rand48.h"
#include "reductions.h"

using namespace LEARNER;
using namespace std;

namespace LDA {

class index_feature {
public:
  uint32_t document;
  feature f;
  bool operator<(const index_feature b) const { return f.weight_index < b.f.weight_index; }
};

  struct lda {
    v_array<float> Elogtheta;
    v_array<float> decay_levels;
    v_array<float> total_new;
    v_array<example* > examples;
    v_array<float> total_lambda;
    v_array<int> doc_lengths;
    v_array<float> digammas;
    v_array<float> v;
    vector<index_feature> sorted_features;

    bool total_lambda_init;

    double example_t;
    vw* all;
  };

#ifdef _WIN32
inline float fmax(float f1, float f2) { return (f1 < f2 ? f2 : f1); }
inline float fmin(float f1, float f2) { return (f1 > f2 ? f2 : f1); }
#endif

#define MINEIRO_SPECIAL
#ifdef MINEIRO_SPECIAL

namespace {

inline float
fastlog2 (float x)
{
  union { float f; uint32_t i; } vx = { x };
  union { uint32_t i; float f; } mx = { (vx.i & 0x007FFFFF) | (0x7e << 23) };
  float y = (float)vx.i;
  y *= 1.0f / (float)(1 << 23);

  return
    y - 124.22544637f - 1.498030302f * mx.f - 1.72587999f / (0.3520887068f + mx.f);
}

inline float
fastlog (float x)
{
  return 0.69314718f * fastlog2 (x);
}

inline float
fastpow2 (float p)
{
  float offset = (p < 0) ? 1.0f : 0.0f;
  float clipp = (p < -126) ? -126.0f : p;
  int w = (int)clipp;
  float z = clipp - w + offset;
  union { uint32_t i; float f; } v = { (uint32_t)((1 << 23) * (clipp + 121.2740838f + 27.7280233f / (4.84252568f - z) - 1.49012907f * z)) };

  return v.f;
}

inline float
fastexp (float p)
{
  return fastpow2 (1.442695040f * p);
}

inline float
fastpow (float x,
         float p)
{
  return fastpow2 (p * fastlog2 (x));
}

inline float
fastlgamma (float x)
{
  float logterm = fastlog (x * (1.0f + x) * (2.0f + x));
  float xp3 = 3.0f + x;

  return
    -2.081061466f - x + 0.0833333f / xp3 - logterm + (2.5f + x) * fastlog (xp3);
}

inline float
fastdigamma (float x)
{
  float twopx = 2.0f + x;
  float logterm = fastlog (twopx);

  return - (1.0f + 2.0f * x) / (x * (1.0f + x))
         - (13.0f + 6.0f * x) / (12.0f * twopx * twopx)
         + logterm;
}

#define log fastlog
#define exp fastexp
#define powf fastpow
#define mydigamma fastdigamma
#define mylgamma fastlgamma

#if defined(__SSE2__) && !defined(VW_LDA_NO_SSE)

#include <emmintrin.h>

typedef __m128 v4sf;
typedef __m128i v4si;

#define v4si_to_v4sf _mm_cvtepi32_ps
#define v4sf_to_v4si _mm_cvttps_epi32

static inline float
v4sf_index (const v4sf x,
            unsigned int i)
{
  union { v4sf f; float array[4]; } tmp = { x };

  return tmp.array[i];
}

static inline const v4sf
v4sfl (float x)
{
  union { float array[4]; v4sf f; } tmp = { { x, x, x, x } };

  return tmp.f;
}

static inline const v4si
v4sil (uint32_t x)
{
  uint64_t wide = (((uint64_t) x) << 32) | x;
  union { uint64_t array[2]; v4si f; } tmp = { { wide, wide } };

  return tmp.f;
}

static inline v4sf
vfastpow2 (const v4sf p)
{
  v4sf ltzero = _mm_cmplt_ps (p, v4sfl (0.0f));
  v4sf offset = _mm_and_ps (ltzero, v4sfl (1.0f));
  v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f));
  v4sf clipp = _mm_andnot_ps (lt126, p) + _mm_and_ps (lt126, v4sfl (-126.0f));
  v4si w = v4sf_to_v4si (clipp);
  v4sf z = clipp - v4si_to_v4sf (w) + offset;

  const v4sf c_121_2740838 = v4sfl (121.2740838f);
  const v4sf c_27_7280233 = v4sfl (27.7280233f);
  const v4sf c_4_84252568 = v4sfl (4.84252568f);
  const v4sf c_1_49012907 = v4sfl (1.49012907f);
  union { v4si i; v4sf f; } v = {
    v4sf_to_v4si (
      v4sfl (1 << 23) *
      (clipp + c_121_2740838 + c_27_7280233 / (c_4_84252568 - z) - c_1_49012907 * z)
    )
  };

  return v.f;
}

inline v4sf
vfastexp (const v4sf p)
{
  const v4sf c_invlog_2 = v4sfl (1.442695040f);

  return vfastpow2 (c_invlog_2 * p);
}

inline v4sf
vfastlog2 (v4sf x)
{
  union { v4sf f; v4si i; } vx = { x };
  union { v4si i; v4sf f; } mx = { (vx.i & v4sil (0x007FFFFF)) | v4sil (0x3f000000) };
  v4sf y = v4si_to_v4sf (vx.i);
  y *= v4sfl (1.1920928955078125e-7f);

  const v4sf c_124_22551499 = v4sfl (124.22551499f);
  const v4sf c_1_498030302 = v4sfl (1.498030302f);
  const v4sf c_1_725877999 = v4sfl (1.72587999f);
  const v4sf c_0_3520087068 = v4sfl (0.3520887068f);

  return y - c_124_22551499
           - c_1_498030302 * mx.f
           - c_1_725877999 / (c_0_3520087068 + mx.f);
}

inline v4sf
vfastlog (v4sf x)
{
  const v4sf c_0_69314718 = v4sfl (0.69314718f);

  return c_0_69314718 * vfastlog2 (x);
}

inline v4sf
vfastdigamma (v4sf x)
{
  v4sf twopx = v4sfl (2.0f) + x;
  v4sf logterm = vfastlog (twopx);

  return (v4sfl (-48.0f) + x * (v4sfl (-157.0f) + x * (v4sfl (-127.0f) - v4sfl (30.0f) * x))) /
         (v4sfl (12.0f) * x * (v4sfl (1.0f) + x) * twopx * twopx)
         + logterm;
}

void
vexpdigammify (vw& all, float* gamma)
{
  unsigned int n = all.lda;
  float extra_sum = 0.0f;
  v4sf sum = v4sfl (0.0f);
  size_t i;

  for (i = 0; i < n && ((uintptr_t) (gamma + i)) % 16 > 0; ++i)
    {
      extra_sum += gamma[i];
      gamma[i] = fastdigamma (gamma[i]);
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (gamma + i);
      sum += arg;
      arg = vfastdigamma (arg);
      _mm_store_ps (gamma + i, arg);
    }

  for (; i < n; ++i)
    {
      extra_sum += gamma[i];
      gamma[i] = fastdigamma (gamma[i]);
    }

  extra_sum += v4sf_index (sum, 0) + v4sf_index (sum, 1) +
               v4sf_index (sum, 2) + v4sf_index (sum, 3);
  extra_sum = fastdigamma (extra_sum);
  sum = v4sfl (extra_sum);

  for (i = 0; i < n && ((uintptr_t) (gamma + i)) % 16 > 0; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (gamma[i] - v4sf_index (sum, 0)));
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (gamma + i);
      arg -= sum;
      arg = vfastexp (arg);
      arg = _mm_max_ps (v4sfl (1e-10f), arg);
      _mm_store_ps (gamma + i, arg);
    }

  for (; i < n; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (gamma[i] - v4sf_index (sum, 0)));
    }
}

void vexpdigammify_2(vw& all, float* gamma, const float* norm)
{
  size_t n = all.lda;
  size_t i;

  for (i = 0; i < n && ((uintptr_t) (gamma + i)) % 16 > 0; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (fastdigamma (gamma[i]) - norm[i]));
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (gamma + i);
      arg = vfastdigamma (arg);
      v4sf vnorm = _mm_loadu_ps (norm + i);
      arg -= vnorm;
      arg = vfastexp (arg);
      arg = _mm_max_ps (v4sfl (1e-10f), arg);
      _mm_store_ps (gamma + i, arg);
    }

  for (; i < n; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (fastdigamma (gamma[i]) - norm[i]));
    }
}

#define myexpdigammify vexpdigammify
#define myexpdigammify_2 vexpdigammify_2

#else
#ifndef _WIN32
#warning "lda IS NOT using sse instructions"
#endif
#define myexpdigammify expdigammify
#define myexpdigammify_2 expdigammify_2

#endif // __SSE2__

} // end anonymous namespace

#else

#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>

using namespace boost::math::policies;

#define mydigamma boost::math::digamma
#define mylgamma boost::math::lgamma
#define myexpdigammify expdigammify
#define myexpdigammify_2 expdigammify_2

#endif // MINEIRO_SPECIAL

float decayfunc(float t, float old_t, float power_t) {
  float result = 1;
  for (float i = old_t+1; i <= t; i += 1)
    result *= (1-powf(i, -power_t));
  return result;
}

float decayfunc2(float t, float old_t, float power_t)
{
  float power_t_plus_one = 1.f - power_t;
  float arg =  - ( powf(t, power_t_plus_one) -
                   powf(old_t, power_t_plus_one));
  return exp ( arg
               / power_t_plus_one);
}

float decayfunc3(double t, double old_t, double power_t)
{
  double power_t_plus_one = 1. - power_t;
  double logt = log((float)t);
  double logoldt = log((float)old_t);
  return (float)((old_t / t) * exp((float)(0.5*power_t_plus_one*(-logt*logt + logoldt*logoldt))));
}

float decayfunc4(double t, double old_t, double power_t)
{
  if (power_t > 0.99)
    return decayfunc3(t, old_t, power_t);
  else
    return (float)decayfunc2((float)t, (float)old_t, (float)power_t);
}

void expdigammify(vw& all, float* gamma)
{
  float sum=0;
  for (size_t i = 0; i<all.lda; i++)
    {
      sum += gamma[i];
      gamma[i] = mydigamma(gamma[i]);
    }
  sum = mydigamma(sum);
  for (size_t i = 0; i<all.lda; i++)
    gamma[i] = fmax(1e-6f, exp(gamma[i] - sum));
}

void expdigammify_2(vw& all, float* gamma, float* norm)
{
  for (size_t i = 0; i<all.lda; i++)
    {
      gamma[i] = fmax(1e-6f, exp(mydigamma(gamma[i]) - norm[i]));
    }
}

float average_diff(vw& all, float* oldgamma, float* newgamma)
{
  float sum = 0.;
  float normalizer = 0.;
  for (size_t i = 0; i<all.lda; i++) {
    sum += fabsf(oldgamma[i] - newgamma[i]);
    normalizer += newgamma[i];
  }
  return sum / normalizer;
}

// Returns E_q[log p(\theta)] - E_q[log q(\theta)].
  float theta_kl(vw& all, v_array<float>& Elogtheta, float* gamma)
{
  float gammasum = 0;
  Elogtheta.erase();
  for (size_t k = 0; k < all.lda; k++) {
    Elogtheta.push_back(mydigamma(gamma[k]));
    gammasum += gamma[k];
  }
  float digammasum = mydigamma(gammasum);
  gammasum = mylgamma(gammasum);
  float kl = -(all.lda*mylgamma(all.lda_alpha));
  kl += mylgamma(all.lda_alpha*all.lda) - gammasum;
  for (size_t k = 0; k < all.lda; k++) {
    Elogtheta[k] -= digammasum;
    kl += (all.lda_alpha - gamma[k]) * Elogtheta[k];
    kl += mylgamma(gamma[k]);
  }

  return kl;
}

float find_cw(vw& all, float* u_for_w, float* v)
{
  float c_w = 0;
  for (size_t k =0; k<all.lda; k++)
    c_w += u_for_w[k]*v[k];

  return 1.f / c_w;
}

v_array<float> new_gamma;
v_array<float> old_gamma;
// Returns an estimate of the part of the variational bound that
// doesn't have to do with beta for the entire corpus for the current
// setting of lambda based on the document passed in. The value is
// divided by the total number of words in the document This can be
// used as a (possibly very noisy) estimate of held-out likelihood.
  float lda_loop(vw& all, v_array<float>& Elogtheta, float* v,weight* weights,example* ec, float power_t)
{
  new_gamma.erase();
  old_gamma.erase();

  for (size_t i = 0; i < all.lda; i++)
    {
      new_gamma.push_back(1.f);
      old_gamma.push_back(0.f);
    }
  size_t num_words =0;
  for (unsigned char* i = ec->indices.begin; i != ec->indices.end; i++)
    num_words += ec->atomics[*i].end - ec->atomics[*i].begin;

  float xc_w = 0;
  float score = 0;
  float doc_length = 0;
  do
    {
      memcpy(v,new_gamma.begin,sizeof(float)*all.lda);
      myexpdigammify(all, v);

      memcpy(old_gamma.begin,new_gamma.begin,sizeof(float)*all.lda);
      memset(new_gamma.begin,0,sizeof(float)*all.lda);

      score = 0;
      size_t word_count = 0;
      doc_length = 0;
      for (unsigned char* i = ec->indices.begin; i != ec->indices.end; i++)
	{
	  feature *f = ec->atomics[*i].begin;
	  for (; f != ec->atomics[*i].end; f++)
	    {
	      float* u_for_w = &weights[(f->weight_index&all.reg.weight_mask)+all.lda+1];
	      float c_w = find_cw(all, u_for_w,v);
	      xc_w = c_w * f->x;
              score += -f->x*log(c_w);
	      size_t max_k = all.lda;
	      for (size_t k =0; k<max_k; k++) {
		new_gamma[k] += xc_w*u_for_w[k];
	      }
	      word_count++;
              doc_length += f->x;
	    }
	}
      for (size_t k =0; k<all.lda; k++)
	new_gamma[k] = new_gamma[k]*v[k]+all.lda_alpha;
    }
  while (average_diff(all, old_gamma.begin, new_gamma.begin) > all.lda_epsilon);

  ec->topic_predictions.erase();
  ec->topic_predictions.resize(all.lda);
  memcpy(ec->topic_predictions.begin,new_gamma.begin,all.lda*sizeof(float));

  score += theta_kl(all, Elogtheta, new_gamma.begin);

  return score / doc_length;
}

size_t next_pow2(size_t x) {
  int i = 0;
  x = x > 0 ? x - 1 : 0;
  while (x > 0) {
    x >>= 1;
    i++;
  }
  return ((size_t)1) << i;
}

void save_load(lda& l, io_buf& model_file, bool read, bool text)
{
  vw* all = l.all;
  uint32_t length = 1 << all->num_bits;
  uint32_t stride = 1 << all->reg.stride_shift;

  if (read)
    {
      initialize_regressor(*all);
      for (size_t j = 0; j < stride*length; j+=stride)
	{
	  for (size_t k = 0; k < all->lda; k++) {
	    if (all->random_weights) {
	      all->reg.weight_vector[j+k] = (float)(-log(frand48()) + 1.0f);
	      all->reg.weight_vector[j+k] *= (float)(all->lda_D / all->lda / all->length() * 200);
	    }
	  }
	  all->reg.weight_vector[j+all->lda] = all->initial_t;
	}
    }

  if (model_file.files.size() > 0)
    {
      uint32_t i = 0;
      uint32_t text_len;
      char buff[512];
      size_t brw = 1;
      do
	{
	  brw = 0;
	  size_t K = all->lda;

	  text_len = sprintf(buff, "%d ", i);
	  brw += bin_text_read_write_fixed(model_file,(char *)&i, sizeof (i),
					   "", read,
					   buff, text_len, text);
	  if (brw != 0)
	    for (uint32_t k = 0; k < K; k++)
	      {
		uint32_t ndx = stride*i+k;

		weight* v = &(all->reg.weight_vector[ndx]);
		text_len = sprintf(buff, "%f ", *v + all->lda_rho);

		brw += bin_text_read_write_fixed(model_file,(char *)v, sizeof (*v),
						 "", read,
						 buff, text_len, text);

	      }
	  if (text)
	    brw += bin_text_read_write_fixed(model_file,buff,0,
					     "", read,
					     "\n",1,text);

	  if (!read)
	    i++;
	}
      while ((!read && i < length) || (read && brw >0));
    }
}

  void learn_batch(lda& l)
  {
    if (l.sorted_features.empty()) {
      // This can happen when the socket connection is dropped by the client.
      // If l.sorted_features is empty, then l.sorted_features[0] does not
      // exist, so we should not try to take its address in the beginning of
      // the for loops down there. Since it seems that there's not much to
      // do in this case, we just return.
      for (size_t d = 0; d < l.examples.size(); d++)
	return_simple_example(*l.all, NULL, *l.examples[d]);
      l.examples.erase();
      return;
    }

    float eta = -1;
    float minuseta = -1;

    if (l.total_lambda.size() == 0)
      {
	for (size_t k = 0; k < l.all->lda; k++)
	  l.total_lambda.push_back(0.f);

	size_t stride = 1 << l.all->reg.stride_shift;
	for (size_t i =0; i <= l.all->reg.weight_mask;i+=stride)
	  for (size_t k = 0; k < l.all->lda; k++)
	    l.total_lambda[k] += l.all->reg.weight_vector[i+k];
      }

    l.example_t++;
    l.total_new.erase();
    for (size_t k = 0; k < l.all->lda; k++)
      l.total_new.push_back(0.f);

    size_t batch_size = l.examples.size();

    sort(l.sorted_features.begin(), l.sorted_features.end());

    eta = l.all->eta * powf((float)l.example_t, - l.all->power_t);
    minuseta = 1.0f - eta;
    eta *= l.all->lda_D / batch_size;
    l.decay_levels.push_back(l.decay_levels.last() + log(minuseta));

    l.digammas.erase();
    float additional = (float)(l.all->length()) * l.all->lda_rho;
    for (size_t i = 0; i<l.all->lda; i++) {
      l.digammas.push_back(mydigamma(l.total_lambda[i] + additional));
    }

    weight* weights = l.all->reg.weight_vector;

    size_t last_weight_index = -1;
    for (index_feature* s = &l.sorted_features[0]; s <= &l.sorted_features.back(); s++)
      {
	if (last_weight_index == s->f.weight_index)
	  continue;
	last_weight_index = s->f.weight_index;
	float* weights_for_w = &(weights[s->f.weight_index & l.all->reg.weight_mask]);
	float decay = fmin(1.0, exp(l.decay_levels.end[-2] - l.decay_levels.end[(int)(-1 - l.example_t+weights_for_w[l.all->lda])]));
	float* u_for_w = weights_for_w + l.all->lda+1;

	weights_for_w[l.all->lda] = (float)l.example_t;
	for (size_t k = 0; k < l.all->lda; k++)
	  {
	    weights_for_w[k] *= decay;
	    u_for_w[k] = weights_for_w[k] + l.all->lda_rho;
	  }
	myexpdigammify_2(*l.all, u_for_w, l.digammas.begin);
      }

    for (size_t d = 0; d < batch_size; d++)
      {
	float score = lda_loop(*l.all, l.Elogtheta, &(l.v[d*l.all->lda]), weights, l.examples[d],l.all->power_t);
	if (l.all->audit)
	  GD::print_audit_features(*l.all, *l.examples[d]);
	// If the doc is empty, give it loss of 0.
	if (l.doc_lengths[d] > 0) {
	  l.all->sd->sum_loss -= score;
	  l.all->sd->sum_loss_since_last_dump -= score;
	}
	return_simple_example(*l.all, NULL, *l.examples[d]);
      }

    for (index_feature* s = &l.sorted_features[0]; s <= &l.sorted_features.back();)
      {
	index_feature* next = s+1;
	while(next <= &l.sorted_features.back() && next->f.weight_index == s->f.weight_index)
	  next++;

	float* word_weights = &(weights[s->f.weight_index & l.all->reg.weight_mask]);
	for (size_t k = 0; k < l.all->lda; k++) {
	  float new_value = minuseta*word_weights[k];
	  word_weights[k] = new_value;
	}

	for (; s != next; s++) {
	  float* v_s = &(l.v[s->document*l.all->lda]);
	  float* u_for_w = &weights[(s->f.weight_index & l.all->reg.weight_mask) + l.all->lda + 1];
	  float c_w = eta*find_cw(*l.all, u_for_w, v_s)*s->f.x;
	  for (size_t k = 0; k < l.all->lda; k++) {
	    float new_value = u_for_w[k]*v_s[k]*c_w;
	    l.total_new[k] += new_value;
	    word_weights[k] += new_value;
	  }
	}
      }
    for (size_t k = 0; k < l.all->lda; k++) {
      l.total_lambda[k] *= minuseta;
      l.total_lambda[k] += l.total_new[k];
    }

    l.sorted_features.resize(0);

    l.examples.erase();
    l.doc_lengths.erase();
  }

  void learn(lda& l, learner& base, example& ec)
  {
    size_t num_ex = l.examples.size();
    l.examples.push_back(&ec);
    l.doc_lengths.push_back(0);
    for (unsigned char* i = ec.indices.begin; i != ec.indices.end; i++) {
      feature* f = ec.atomics[*i].begin;
      for (; f != ec.atomics[*i].end; f++) {
	index_feature temp = {(uint32_t)num_ex, *f};
	l.sorted_features.push_back(temp);
	l.doc_lengths[num_ex] += (int)f->x;
      }
    }
    if (++num_ex == l.all->minibatch)
      learn_batch(l);
  }

  // placeholder
  void predict(lda& l, learner& base, example& ec)
  {
    bool test_only = ec.test_only;
    ec.test_only = true;
    learn(l, base, ec);
    ec.test_only = test_only;
  }

  void end_pass(lda& l)
  {
    if (l.examples.size())
      learn_batch(l);
  }

void end_examples(lda& l)
{
  for (size_t i = 0; i < l.all->length(); i++) {
    weight* weights_for_w = & (l.all->reg.weight_vector[i << l.all->reg.stride_shift]);
    float decay = fmin(1.0, exp(l.decay_levels.last() - l.decay_levels.end[(int)(-1- l.example_t +weights_for_w[l.all->lda])]));
    for (size_t k = 0; k < l.all->lda; k++)
      weights_for_w[k] *= decay;
  }
}

  void finish_example(vw& all, lda&, example& ec)
{}

  void finish(lda& ld)
  {
    ld.sorted_features.~vector<index_feature>();
    ld.Elogtheta.delete_v();
    ld.decay_levels.delete_v();
    ld.total_new.delete_v();
    ld.examples.delete_v();
    ld.total_lambda.delete_v();
    ld.doc_lengths.delete_v();
    ld.digammas.delete_v();
    ld.v.delete_v();
  }

learner* setup(vw&all, vector<string>&opts, po::variables_map& vm)
{
  lda* ld = (lda*)calloc_or_die(1,sizeof(lda));
  ld->sorted_features = vector<index_feature>();
  ld->total_lambda_init = 0;
  ld->all = &all;
  ld->example_t = all.initial_t;

  po::options_description desc("LDA options");
  desc.add_options()
    ("lda_alpha", po::value<float>(&all.lda_alpha), "Prior on sparsity of per-document topic weights")
    ("lda_rho", po::value<float>(&all.lda_rho), "Prior on sparsity of topic distributions")
    ("lda_D", po::value<float>(&all.lda_D), "Number of documents")
    ("lda_epsilon", po::value<float>(&all.lda_epsilon), "Loop convergence threshold")
    ("minibatch", po::value<size_t>(&all.minibatch), "Minibatch size, for LDA");

  po::parsed_options parsed = po::command_line_parser(opts).
    style(po::command_line_style::default_style ^ po::command_line_style::allow_guessing).
    options(desc).allow_unregistered().run();
  opts = po::collect_unrecognized(parsed.options, po::include_positional);
  po::store(parsed, vm);
  po::notify(vm);

  all.p->sort_features = true;
  float temp = ceilf(logf((float)(all.lda*2+1)) / logf (2.f));
  all.reg.stride_shift = (size_t)temp;
  all.random_weights = true;
  all.add_constant = false;

  if (vm.count("lda") && all.eta > 1.)
    {
      cerr << "your learning rate is too high, setting it to 1" << endl;
      all.eta = min(all.eta,1.f);
    }

  if (vm.count("minibatch")) {
    size_t minibatch2 = next_pow2(all.minibatch);
    all.p->ring_size = all.p->ring_size > minibatch2 ? all.p->ring_size : minibatch2;
  }

  ld->v.resize(all.lda*all.minibatch);

  ld->decay_levels.push_back(0.f);

  all.l->finish();
  learner* l = new learner(ld, 1 << all.reg.stride_shift);
  l->set_learn<lda,learn>();
  l->set_predict<lda,predict>();
  l->set_save_load<lda,save_load>();
  l->set_finish_example<lda,finish_example>();
  l->set_end_examples<lda,end_examples>();
  l->set_end_pass<lda,end_pass>();
  l->set_finish<lda,finish>();

  return l;
}
}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */
#ifndef SP_ALGO_NN_LOSS_GRADIENT_HPP
#define SP_ALGO_NN_LOSS_GRADIENT_HPP

#include <boost/assert.hpp>
#include "../config.hpp"
#include "../matrix.hpp"
#include "../types.hpp"
#include "sp/util/hints.hpp"

SP_ALGO_NN_NAMESPACE_BEGIN

/**
 * \file Gradient helper funciton
 */

/**
 * \brief Calculate the the gradient of the expected vs actual output of
 *        a mini-batch
 */
template<typename LossFunction>
void gradient(LossFunction& loss, tensor_4& predicted, tensor_4& expected, tensor_4& result) {
    BOOST_ASSERT_MSG(
        predicted.dimensions() == expected.dimensions(),
        "Dimensions of Expected and Actual must match"
    );
    const size_t sample_count = predicted.dimension(0);
    for(size_t si = 0; si < sample_count; ++si) {
        loss.derivative(si, predicted, expected, result);
    }
}

SP_ALGO_NN_NAMESPACE_END

#endif	/* SP_ALGO_NN_LOSS_GRADIENT_HPP */


/* Copyright (c) 2016 - 2019, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef ELECTRON_BEAM_TEMPLATES_HH
#define ELECTRON_BEAM_TEMPLATES_HH

#include <ElectronBeam.hh>
#include <instantiation.hh>
#include <utils.hh>

#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>

#include <cstdlib>

using std::pow;

namespace adamantine
{
namespace internal
{
class PointSource : public dealii::Function<1>
{
public:
  PointSource(std::vector<double> const &position);

  double value(dealii::Point<1> const &time,
               unsigned int const component = 0) const override;

  void rewind_time();

  void save_time();

private:
  mutable unsigned int _current_pos;
  unsigned int _saved_pos;
  mutable dealii::Point<1> _current_time;
  dealii::Point<1> _saved_time;
  std::vector<double> _position;
};

PointSource::PointSource(std::vector<double> const &position)
    : _current_pos(-1), _saved_pos(-1), _position(position)
{
  _current_time[0] = -1.;
}

double PointSource::value(dealii::Point<1> const &time,
                          unsigned int const) const
{
  // If the time is greater than the current one, we use the next entry in the
  // vector.
  if (time[0] > _current_time[0])
  {
    ++_current_pos;
    _current_time[0] = time[0];
  }

  return _position[_current_pos];
}

void PointSource::rewind_time()
{
  _current_pos = _saved_pos;
  _current_time = _saved_time;
}

void PointSource::save_time()
{
  _saved_pos = _current_pos;
  _saved_time = _current_time;
}
} // namespace internal

template <int dim>
ElectronBeam<dim>::ElectronBeam(boost::property_tree::ptree const &database)
    : dealii::Function<dim>(), _is_point_source(false), _max_height(0.)
{
  // Set the properties of the electron beam.
  _beam.depth = database.get<double>("depth");
  _beam.energy_conversion_eff =
      database.get<double>("energy_conversion_efficiency");
  _beam.control_eff = database.get<double>("control_efficiency");
  _beam.diameter_squared = pow(database.get("diameter", 2e-3), 2);
  boost::optional<double> max_power =
      database.get_optional<double>("max_power");
  if (max_power)
    _beam.max_power = max_power.get();
  else
  {
    double const current = database.get<double>("current");
    double const voltage = database.get<double>("voltage");
    _beam.max_power = current * voltage;
  }

  // The only variable that can be used to define the position is the time t.
  std::string variable = "t";
  // Predefined constants
  std::map<std::string, double> constants;
  constants["pi"] = dealii::numbers::PI;

  boost::optional<std::string> input_file =
      database.get_optional<std::string>("input_file");
  if (input_file)
  {
    std::array<std::vector<double>, dim - 1> points;
    std::string delimiter = database.get<std::string>("delimiter");
    ASSERT_THROW(boost::filesystem::exists(input_file.get()) == true,
                 "The file " + input_file.get() + " does not exist.");
    std::ifstream file(input_file.get());
    std::string line;
    // Read the file line by line, split the strings, and convert the strings
    // into double.
    if (file)
    {
      while (std::getline(file, line))
      {
        std::vector<std::string> split_strings;
        boost::algorithm::split(split_strings, line,
                                boost::is_any_of(delimiter),
                                boost::algorithm::token_compress_on);
        ASSERT_THROW(split_strings.size() == dim - 1,
                     "Problem parsing the source input file.");
        for (unsigned int i = 0; i < dim - 1; ++i)
          points[i].push_back(std::atof(split_strings[i].c_str()));
      }
      file.close();
    }

    // Create the point source.
    for (unsigned int i = 0; i < dim - 1; ++i)
      _position[i].reset(new internal::PointSource(points[i]));
    _is_point_source = true;
  }
  else
  {
    std::array<std::string, 2> position_expression = {{"abscissa", "ordinate"}};
    for (unsigned int i = 0; i < dim - 1; ++i)
    {
      std::string expression =
          database.get<std::string>(position_expression[i]);
      _position[i].reset(new dealii::FunctionParser<1>());
      static_cast<dealii::FunctionParser<1> *>(_position[i].get())
          ->initialize(variable, expression, constants);
    }
  }
}

template <int dim>
void ElectronBeam<dim>::rewind_time()
{
  if (_is_point_source)
    for (unsigned int i = 0; i < dim - 1; ++i)
      static_cast<internal::PointSource *>(_position[i].get())->rewind_time();
}

template <int dim>
void ElectronBeam<dim>::save_time()
{
  if (_is_point_source)
    for (unsigned int i = 0; i < dim - 1; ++i)
      static_cast<internal::PointSource *>(_position[i].get())->save_time();
}

template <int dim>
double ElectronBeam<dim>::value(dealii::Point<dim> const &point,
                                unsigned int const /*component*/) const
{
  double const z = point[1] - _max_height;
  if ((z + _beam.depth) < 0.)
    return 0.;
  else
  {
    double const distribution_z =
        -3. * pow(z / _beam.depth, 2) - 2. * (z / _beam.depth) + 1.;

    dealii::Point<1> time;
    time[0] = this->get_time();
    double const beam_center_x = _position[0]->value(time);
    double xpy_squared = pow(point[0] - beam_center_x, 2);
    if (dim == 3)
    {
      double const beam_center_y = _position[1]->value(time);
      xpy_squared += pow(point[2] - beam_center_y, 2);
    }

    double constexpr four_ln_pone = 4. * std::log(0.1);
    double heat_source = 0.;
    heat_source =
        -_beam.energy_conversion_eff * _beam.control_eff * _beam.max_power *
        four_ln_pone /
        (dealii::numbers::PI * _beam.diameter_squared * _beam.depth) *
        std::exp(four_ln_pone * xpy_squared / _beam.diameter_squared) *
        distribution_z;

    return heat_source;
  }
}
} // namespace adamantine

INSTANTIATE_DIM(ElectronBeam)

#endif


#include <algorithm>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <map>
#include <numeric>
#include <sstream>
#include <stdint.h>
#include <string>
#include <vector>
#include <boost/regex.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <tr1/unordered_map>
#include <boost/function.hpp>

using namespace std;
using namespace boost;
using namespace boost::lambda;
using namespace std::tr1;

typedef unordered_map<char, string> ProductionMap;
class DOL
{
  public:
    static regex matchProduction;

    DOL(const std::string& source)
    {
      std::vector<std::string> strs;
      boost::split(strs, source, boost::is_any_of("\n"));
      _axiom = strs[0];
      for_each(strs.begin()+1, strs.end(), bind(&DOL::parseLine, this, cref(_1)));
    }

    const ProductionMap& productions()const { return _productions; }
    const string& axiom()const { return _axiom; }

    std::string& step(std::string& str)const
    {
      string result;
      for (std::string::const_iterator i = str.begin();
          i != str.end(); ++i)
      {
        ProductionMap::const_iterator p = _productions.find(*i);
        if (p != _productions.end())
        {
          result += p->second;
        }
        else
        {
          result += *i;
        }
      }
      str = result;
      return str;
    }

    private:
    void parseLine(const std::string& line)
    {
      cmatch what;
      if(regex_match(line.c_str(), what, matchProduction))
      {
        string succ(what[2]);
        char c = *(what[1].first);
        _productions[c] = succ;
      }
    }
    std::string _axiom;
    ProductionMap _productions;
};

regex DOL::matchProduction("(\\w)\\s*->\\s*(.+)\\s*$");

std::ostream& operator<<(std::ostream& os, const DOL& rhs)
{
  os << "axiom: " << rhs.axiom() << '\n';
  os << "productions:\n";
  for_each(rhs.productions().begin(), rhs.productions().end(),
      os << bind(&ProductionMap::value_type::first, _1) << ": "
      << bind(&ProductionMap::value_type::second, _1) << '\n');
  return os;
}

class Turtle;
typedef boost::function<void(Turtle*)> Action;
typedef std::tr1::unordered_map<char, Action> ActionMap;
class Turtle
{
  public:
    Turtle()
    {
      _actions['F'] = &Turtle::draw;
      _actions['f'] = &Turtle::forward;
      _actions['+'] = &Turtle::left;
      _actions['-'] = &Turtle::right;
    }

    void render(const std::string& program)
    {
      for_each(program.begin(), program.end(),
        bind(&Turtle::doAction, this, _1));
    }
  private:
    void doAction(const char a) { _actions[a](this); }
    void draw() { cout << "draw "; }
    void forward() { cout << "forward "; }
    void left() { cout << "left "; }
    void right() { cout << "right "; }

    ActionMap _actions;
};

int main(int argc, char **argv)
{
  string ts = "F-F-F-F\n"
    "F -> F-F+F+FF-F-F+F\n";

  DOL dol(ts);
  cout << "dol: " << dol << endl;

  string q(dol.axiom());
  Turtle turtle;
  for (int i = 0; i < 1; ++i)
  {
    cout << dol.step(q) << endl;
    turtle.render(q);
  }
  return 0;
}