/*
 * Copyright 2010,
 * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
 *
 * CNRS
 *
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* SOT */
#define ENABLE_RT_LOG

#include "sot/core/device.hh"
#include <iostream>
#include <sot/core/debug.hh>
using namespace std;

#include <Eigen/Geometry>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/real-time-logger.h>
#include <sot/core/matrix-geometry.hh>

#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;

const std::string Device::CLASS_NAME = "Device";

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

void Device::integrateRollPitchYaw(Vector &state, const Vector &control,
                                   double dt) {
  using Eigen::AngleAxisd;
  using Eigen::QuaternionMapd;
  using Eigen::Vector3d;

  typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
  Eigen::Matrix<double, 7, 1> qin, qout;
  qin.head<3>() = state.head<3>();

  QuaternionMapd quat(qin.tail<4>().data());
  quat = AngleAxisd(state(5), Vector3d::UnitZ()) *
         AngleAxisd(state(4), Vector3d::UnitY()) *
         AngleAxisd(state(3), Vector3d::UnitX());

  SE3().integrate(qin, control.head<6>() * dt, qout);

  // Update freeflyer pose
  ffPose_.translation() = qout.head<3>();
  state.head<3>() = qout.head<3>();

  ffPose_.linear() = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
  state.segment<3>(3) = ffPose_.linear().eulerAngles(2, 1, 0).reverse();
}

const MatrixHomogeneous &Device::freeFlyerPose() const { return ffPose_; }

Device::~Device() {
  for (unsigned int i = 0; i < 4; ++i) {
    delete forcesSOUT[i];
  }
}

Device::Device(const std::string &n)
    : Entity(n), state_(6), sanityCheck_(true),
      controlInputType_(CONTROL_INPUT_ONE_INTEGRATION),
      controlSIN(NULL, "Device(" + n + ")::input(double)::control"),
      attitudeSIN(NULL, "Device(" + n + ")::input(vector3)::attitudeIN"),
      zmpSIN(NULL, "Device(" + n + ")::input(vector3)::zmp"),
      stateSOUT("Device(" + n + ")::output(vector)::state")
      //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
      ,
      velocitySOUT("Device(" + n + ")::output(vector)::velocity"),
      attitudeSOUT("Device(" + n + ")::output(matrixRot)::attitude"),
      motorcontrolSOUT("Device(" + n + ")::output(vector)::motorcontrol"),
      previousControlSOUT("Device(" + n + ")::output(vector)::previousControl"),
      ZMPPreviousControllerSOUT("Device(" + n +
                                ")::output(vector)::zmppreviouscontroller"),
      robotState_("Device(" + n + ")::output(vector)::robotState"),
      robotVelocity_("Device(" + n + ")::output(vector)::robotVelocity"),
      pseudoTorqueSOUT("Device(" + n + ")::output(vector)::ptorque")

      ,
      ffPose_(), forceZero6(6) {
  forceZero6.fill(0);
  /* --- SIGNALS --- */
  for (int i = 0; i < 4; ++i) {
    withForceSignals[i] = false;
  }
  forcesSOUT[0] =
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRLEG");
  forcesSOUT[1] =
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLLEG");
  forcesSOUT[2] =
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRARM");
  forcesSOUT[3] =
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLARM");

  signalRegistration(
      controlSIN << stateSOUT << robotState_ << robotVelocity_ << velocitySOUT
                 << attitudeSOUT << attitudeSIN << zmpSIN << *forcesSOUT[0]
                 << *forcesSOUT[1] << *forcesSOUT[2] << *forcesSOUT[3]
                 << previousControlSOUT << pseudoTorqueSOUT << motorcontrolSOUT
                 << ZMPPreviousControllerSOUT);
  state_.fill(.0);
  stateSOUT.setConstant(state_);

  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant(velocity_);

  /* --- Commands --- */
  {
    std::string docstring;
    /* Command setStateSize. */
    docstring = "\n"
                "    Set size of state vector\n"
                "\n";
    addCommand("resize", new command::Setter<Device, unsigned int>(
                             *this, &Device::setStateSize, docstring));
    docstring = "\n"
                "    Set state vector value\n"
                "\n";
    addCommand("set", new command::Setter<Device, Vector>(
                          *this, &Device::setState, docstring));

    docstring = "\n"
                "    Set velocity vector value\n"
                "\n";
    addCommand("setVelocity", new command::Setter<Device, Vector>(
                                  *this, &Device::setVelocity, docstring));

    void (Device::*setRootPtr)(const Matrix &) = &Device::setRoot;
    docstring = command::docCommandVoid1("Set the root position.",
                                         "matrix homogeneous");
    addCommand("setRoot",
               command::makeCommandVoid1(*this, setRootPtr, docstring));

    /* Second Order Integration set. */
    docstring = "\n"
                "    Set the position calculous starting from  \n"
                "    acceleration measure instead of velocity \n"
                "\n";

    addCommand("setSecondOrderIntegration",
               command::makeCommandVoid0(
                   *this, &Device::setSecondOrderIntegration, docstring));

    /* Display information */
    docstring = "\n"
                "    Display information on device  \n"
                "\n";
    addCommand("display", command::makeCommandVoid0(*this, &Device::cmdDisplay,
                                                    docstring));

    /* SET of control input type. */
    docstring = "\n"
                "    Set the type of control input which can be  \n"
                "    acceleration, velocity, or position\n"
                "\n";

    addCommand("setControlInputType",
               new command::Setter<Device, string>(
                   *this, &Device::setControlInputType, docstring));

    docstring = "\n"
                "    Enable/Disable sanity checks\n"
                "\n";
    addCommand("setSanityCheck",
               new command::Setter<Device, bool>(*this, &Device::setSanityCheck,
                                                 docstring));

    addCommand("setPositionBounds",
               command::makeCommandVoid2(
                   *this, &Device::setPositionBounds,
                   command::docCommandVoid2("Set robot position bounds",
                                            "vector: lower bounds",
                                            "vector: upper bounds")));

    addCommand("setVelocityBounds",
               command::makeCommandVoid2(
                   *this, &Device::setVelocityBounds,
                   command::docCommandVoid2("Set robot velocity bounds",
                                            "vector: lower bounds",
                                            "vector: upper bounds")));

    addCommand("setTorqueBounds",
               command::makeCommandVoid2(
                   *this, &Device::setTorqueBounds,
                   command::docCommandVoid2("Set robot torque bounds",
                                            "vector: lower bounds",
                                            "vector: upper bounds")));

    addCommand("getTimeStep",
               command::makeDirectGetter(
                   *this, &this->timestep_,
                   command::docDirectGetter("Time step", "double")));

    // Handle commands and signals called in a synchronous way.
    periodicCallBefore_.addSpecificCommands(*this, commandMap, "before.");
    periodicCallAfter_.addSpecificCommands(*this, commandMap, "after.");
  }
}

void Device::setStateSize(const unsigned int &size) {
  state_.resize(size);
  state_.fill(.0);
  stateSOUT.setConstant(state_);
  previousControlSOUT.setConstant(state_);
  pseudoTorqueSOUT.setConstant(state_);
  motorcontrolSOUT.setConstant(state_);

  Device::setVelocitySize(size);

  Vector zmp(3);
  zmp.fill(.0);
  ZMPPreviousControllerSOUT.setConstant(zmp);
}

void Device::setVelocitySize(const unsigned int &size) {
  velocity_.resize(size);
  velocity_.fill(.0);
  velocitySOUT.setConstant(velocity_);
}

void Device::setState(const Vector &st) {
  if (sanityCheck_) {
    const Vector::Index &s = st.size();
    switch (controlInputType_) {
    case CONTROL_INPUT_TWO_INTEGRATION:
      dgRTLOG()
          << "Sanity check for this control is not well supported. "
             "In order to make it work, use pinocchio and the contact forces "
             "to estimate the joint torques for the given acceleration.\n";
      if (s != lowerTorque_.size() || s != upperTorque_.size())
        throw std::invalid_argument(
            "Upper and/or lower torque bounds "
            "do not match state size. Set them first with setTorqueBounds");
      // fall through
    case CONTROL_INPUT_ONE_INTEGRATION:
      if (s != lowerVelocity_.size() || s != upperVelocity_.size())
        throw std::invalid_argument("Upper and/or lower velocity bounds "
                                    "do not match state size."
                                    " Set them first with setVelocityBounds");
      // fall through
    case CONTROL_INPUT_NO_INTEGRATION:
      break;
    default:
      throw std::invalid_argument("Invalid control mode type.");
    }
  }
  state_ = st;
  stateSOUT.setConstant(state_);
  motorcontrolSOUT.setConstant(state_);
}

void Device::setVelocity(const Vector &vel) {
  velocity_ = vel;
  velocitySOUT.setConstant(velocity_);
}

void Device::setRoot(const Matrix &root) {
  Eigen::Matrix4d _matrix4d(root);
  MatrixHomogeneous _root(_matrix4d);
  setRoot(_root);
}

void Device::setRoot(const MatrixHomogeneous &worldMwaist) {
  VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2, 1, 0)).reverse();
  Vector q = state_;
  q = worldMwaist.translation(); // abusive ... but working.
  for (unsigned int i = 0; i < 3; ++i)
    q(i + 3) = r(i);
}

void Device::setSecondOrderIntegration() {
  controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant(velocity_);
}

void Device::setNoIntegration() {
  controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant(velocity_);
}

void Device::setControlInputType(const std::string &cit) {
  for (int i = 0; i < CONTROL_INPUT_SIZE; i++)
    if (cit == ControlInput_s[i]) {
      controlInputType_ = (ControlInput)i;
      sotDEBUG(25) << "Control input type: " << ControlInput_s[i] << endl;
      return;
    }
  sotDEBUG(25) << "Unrecognized control input type: " << cit << endl;
}

void Device::setSanityCheck(const bool &enableCheck) {
  if (enableCheck) {
    const Vector::Index &s = state_.size();
    switch (controlInputType_) {
    case CONTROL_INPUT_TWO_INTEGRATION:
      dgRTLOG()
          << "Sanity check for this control is not well supported. "
             "In order to make it work, use pinocchio and the contact forces "
             "to estimate the joint torques for the given acceleration.\n";
      if (s != lowerTorque_.size() || s != upperTorque_.size())
        throw std::invalid_argument(
            "Upper and/or lower torque bounds "
            "do not match state size. Set them first with setTorqueBounds");
      // fall through
    case CONTROL_INPUT_ONE_INTEGRATION:
      if (s != lowerVelocity_.size() || s != upperVelocity_.size())
        throw std::invalid_argument(
            "Upper and/or lower velocity bounds "
            "do not match state size. Set them first with setVelocityBounds");
      // fall through
    case CONTROL_INPUT_NO_INTEGRATION:
      if (s != lowerPosition_.size() || s != upperPosition_.size())
        throw std::invalid_argument(
            "Upper and/or lower position bounds "
            "do not match state size. Set them first with setPositionBounds");
      break;
    default:
      throw std::invalid_argument("Invalid control mode type.");
    }
  }
  sanityCheck_ = enableCheck;
}

void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
  std::ostringstream oss;
  if (lower.size() != state_.size()) {
    oss << "Lower bound size should be " << state_.size();
    throw std::invalid_argument(oss.str());
  }
  if (upper.size() != state_.size()) {
    oss << "Upper bound size should be " << state_.size();
    throw std::invalid_argument(oss.str());
  }
  lowerPosition_ = lower;
  upperPosition_ = upper;
}

void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
  std::ostringstream oss;
  if (lower.size() != velocity_.size()) {
    oss << "Lower bound size should be " << velocity_.size();
    throw std::invalid_argument(oss.str());
  }
  if (upper.size() != velocity_.size()) {
    oss << "Upper bound size should be " << velocity_.size();
    throw std::invalid_argument(oss.str());
  }
  lowerVelocity_ = lower;
  upperVelocity_ = upper;
}

void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
  // TODO I think the torque bounds size are state_.size()-6...
  std::ostringstream oss;
  if (lower.size() != state_.size()) {
    oss << "Lower bound size should be " << state_.size();
    throw std::invalid_argument(oss.str());
  }
  if (upper.size() != state_.size()) {
    oss << "Lower bound size should be " << state_.size();
    throw std::invalid_argument(oss.str());
  }
  lowerTorque_ = lower;
  upperTorque_ = upper;
}

void Device::increment(const double &dt) {
  int time = stateSOUT.getTime();
  sotDEBUG(25) << "Time : " << time << std::endl;

  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
  try {
    periodicCallBefore_.run(time + 1);
  } catch (std::exception &e) {
    dgRTLOG() << "exception caught while running periodical commands (before): "
              << e.what() << std::endl;
  } catch (const char *str) {
    dgRTLOG() << "exception caught while running periodical commands (before): "
              << str << std::endl;
  } catch (...) {
    dgRTLOG() << "unknown exception caught while"
              << " running periodical commands (before)" << std::endl;
  }

  /* Force the recomputation of the control. */
  controlSIN(time);
  sotDEBUG(25) << "u" << time << " = " << controlSIN.accessCopy() << endl;

  /* Integration of numerical values. This function is virtual. */
  integrate(dt);
  sotDEBUG(25) << "q" << time << " = " << state_ << endl;

  /* Position the signals corresponding to sensors. */
  stateSOUT.setConstant(state_);
  stateSOUT.setTime(time + 1);
  // computation of the velocity signal
  if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
    velocitySOUT.setConstant(velocity_);
    velocitySOUT.setTime(time + 1);
  } else if (controlInputType_ == CONTROL_INPUT_ONE_INTEGRATION) {
    velocitySOUT.setConstant(controlSIN.accessCopy());
    velocitySOUT.setTime(time + 1);
  }
  for (int i = 0; i < 4; ++i) {
    if (!withForceSignals[i])
      forcesSOUT[i]->setConstant(forceZero6);
  }
  Vector zmp(3);
  zmp.fill(.0);
  ZMPPreviousControllerSOUT.setConstant(zmp);

  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
  try {
    periodicCallAfter_.run(time + 1);
  } catch (std::exception &e) {
    dgRTLOG() << "exception caught while running periodical commands (after): "
              << e.what() << std::endl;
  } catch (const char *str) {
    dgRTLOG() << "exception caught while running periodical commands (after): "
              << str << std::endl;
  } catch (...) {
    dgRTLOG() << "unknown exception caught while"
              << " running periodical commands (after)" << std::endl;
  }

  // Others signals.
  motorcontrolSOUT.setConstant(state_);
}

// Return true if it saturates.
inline bool saturateBounds(double &val, const double &lower,
                           const double &upper) {
  assert(lower <= upper);
  if (val < lower) {
    val = lower;
    return true;
  }
  if (upper < val) {
    val = upper;
    return true;
  }
  return false;
}

#define CHECK_BOUNDS(val, lower, upper, what)                                  \
  for (int i = 0; i < val.size(); ++i) {                                       \
    double old = val(i);                                                       \
    if (saturateBounds(val(i), lower(i), upper(i))) {                          \
      std::ostringstream oss;                                                  \
      oss << "Robot " what " bound violation at DoF " << i << ": requested "   \
          << old << " but set " << val(i) << '\n';                             \
      SEND_ERROR_STREAM_MSG(oss.str());                                        \
    }                                                                          \
  }

void Device::integrate(const double &dt) {
  const Vector &controlIN = controlSIN.accessCopy();

  if (sanityCheck_ && controlIN.hasNaN()) {
    dgRTLOG() << "Device::integrate: Control has NaN values: " << '\n'
              << controlIN.transpose() << '\n';
    return;
  }

  if (controlInputType_ == CONTROL_INPUT_NO_INTEGRATION) {
    state_.tail(controlIN.size()) = controlIN;
    return;
  }

  if (vel_control_.size() == 0)
    vel_control_ = Vector::Zero(controlIN.size());

  // If control size is state size - 6, integrate joint angles,
  // if control and state are of same size, integrate 6 first degrees of
  // freedom as a translation and roll pitch yaw.

  if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
    // TODO check acceleration
    // Position increment
    vel_control_ = velocity_.tail(controlIN.size()) + (0.5 * dt) * controlIN;
    // Velocity integration.
    velocity_.tail(controlIN.size()) += controlIN * dt;
  } else {
    vel_control_ = controlIN;
  }

  // Velocity bounds check
  if (sanityCheck_) {
    CHECK_BOUNDS(velocity_, lowerVelocity_, upperVelocity_, "velocity");
  }

  if (vel_control_.size() == state_.size()) {
    // Freeflyer integration
    integrateRollPitchYaw(state_, vel_control_, dt);
    // Joints integration
    state_.tail(state_.size() - 6) += vel_control_.tail(state_.size() - 6) * dt;
  } else {
    // Position integration
    state_.tail(controlIN.size()) += vel_control_ * dt;
  }

  // Position bounds check
  if (sanityCheck_) {
    CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position");
  }
}

/* --- DISPLAY ------------------------------------------------------------ */

void Device::display(std::ostream &os) const {
  os << name << ": " << state_ << endl
     << "sanityCheck: " << sanityCheck_ << endl
     << "controlInputType:" << controlInputType_ << endl;
}

void Device::cmdDisplay() {
  std::cout << name << ": " << state_ << endl
            << "sanityCheck: " << sanityCheck_ << endl
            << "controlInputType:" << controlInputType_ << endl;
}


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_DEFINITION_TEN_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_TEN_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct ten_ : boost::dispatch::constant_value_<ten_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,ten_,boost::dispatch::constant_value_<ten_>);
      BOOST_SIMD_REGISTER_CONSTANT(10, 0x41200000UL, 0x4024000000000000ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,ten_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::ten_,ten);
  }

  template<typename T> BOOST_FORCEINLINE auto Ten()
  BOOST_NOEXCEPT_DECLTYPE(detail::ten( boost::dispatch::as_<T>{}))
  {
    return detail::ten( boost::dispatch::as_<T>{} );
  }
} }

#endif


//
// this is for testing the timing of the given individual integrals
//

#include "libgen.h"
#include <boost/algorithm/string.hpp>   // string handling
#include <boost/lexical_cast.hpp>
#include <boost/timer.hpp>
using namespace boost;

extern void hgp_os_eri_sp_sp_sp_sp(const UInt& inp2, const UInt& jnp2, const Double& pMax,
		const Double& omega, const Double* icoe, const Double* iexp, const Double* ifac,
		const Double* P, const Double* A, const Double* B, const Double* jcoe, const Double* jexp,
		const Double* jfac, const Double* Q, const Double* C, const Double* D, Double* abcd);

extern void hgp_os_eri_sp_sp_sp_sp_d1(const UInt& inp2, const UInt& jnp2, const Double& pMax,
		const Double& omega, const Double* icoe, const Double* iexp, const Double* iexpdiff,
		const Double* ifac, const Double* P, const Double* A, const Double* B, const Double* jcoe,
		const Double* jexp, const Double* jexpdiff, const Double* jfac, const Double* Q, const Double* C,
		const Double* D, Double* abcd);

Int main(int argc, char* argv[])
{
	/////////////////////////////////////////////////////////////////////////////
	// setting the shell data
	// all of shell data are in default to be composite shell
	// which contains two sub-shells
	/////////////////////////////////////////////////////////////////////////////

	// shell 1
	Int inp = 2;
	vector<Double> iexp(inp);
	vector<Double> icoe(2*inp);
	iexp[0] = 0.1812885;
	iexp[1] = 0.0442493;
	icoe[0] = 0.1193324;
	icoe[1] = 0.1608542;
	icoe[2] = 0.8434564;
	icoe[3] = 0.0689991;
	Double A[3];
	A[0]    = 1.0;
	A[1]    = 0.0;
	A[2]    = 0.0;

	// shell 2
	Int jnp = 2;
	vector<Double> jexp(jnp);
	vector<Double> jcoe(2*jnp);
	jexp[0] = 0.1439130;
	jexp[1] = 0.0914760;
	jcoe[0] = 6.139703E-02;
	jcoe[1] = 3.061130E-01;
	jcoe[2] = 1.154890;
	jcoe[3] = 0.1891265;
	Double B[3];
	B[0]    = 0.0;
	B[1]    = 1.0;
	B[2]    = 0.0;

	// shell 3
	Int knp = 2;
	vector<Double> kexp(knp);
	vector<Double> kcoe(2*knp);
	kexp[0] = 0.03628970;
	kexp[1] = 0.14786010;
	kcoe[0] = 0.09996723;
	kcoe[1] = 0.39951283;
	kcoe[2] = 0.70011547;
	kcoe[3] = 0.15591627;
	Double C[3];
	C[0]    = 0.0;
	C[1]    = 0.0;
	C[2]    = 1.0;

	// shell 4
	Int lnp = 2;
	vector<Double> lexp(lnp);
	vector<Double> lcoe(2*lnp);
	lexp[0] = 0.01982050;
	lexp[1] = 0.16906180;
	lcoe[0] = 0.06723;
	lcoe[1] = 0.51283;
	lcoe[2] = 0.30011547;
	lcoe[3] = 0.5591627;
	Double D[3];
	D[0]    = 0.0;
	D[1]    = 1.0;
	D[2]    = 1.0;

	/////////////////////////////////////////////////////////////////////////////
	// setting the shell data
	// all of shell data are in default to be composite shell
	// which contains two sub-shells
	/////////////////////////////////////////////////////////////////////////////
	//
	// form the data for hgp calculation
	// firstly it's the bra side
	//
	Double AB2 = (A[0]-B[0])*(A[0]-B[0])+(A[1]-B[1])*(A[1]-B[1])+(A[2]-B[2])*(A[2]-B[2]);
	Int inp2 = inp*jnp;
	vector<Double> iexp2(inp2,ZERO);
	vector<Double> fbra(inp2,ZERO);
	vector<Double> P(3*inp2,ZERO);
	vector<Double> iexpdiff(inp2,ZERO);
	Int count = 0;
	for(Int jp=0; jp<jnp; jp++) {
		for(Int ip=0; ip<inp; ip++) {

			// prefactors etc.
			Double ia   = iexp[ip];
			Double ja   = jexp[jp];
			Double alpla= ia+ja;
			Double diff = ia-ja;
			Double ab   = -ia*ja/alpla;
			Double pref = exp(ab*AB2)*pow(PI/alpla,1.5E0);
			iexp2[count]= ONE/alpla;
			fbra[count] = pref;
			iexpdiff[count]= diff;

			// form P point according to the
			// Gaussian pritimive product theorem
			Double adab = ia/alpla;
			Double bdab = ja/alpla;
			Double Px   = A[0]*adab + B[0]*bdab;
			Double Py   = A[1]*adab + B[1]*bdab;
			Double Pz   = A[2]*adab + B[2]*bdab;
			P[3*count+0]= Px;
			P[3*count+1]= Py;
			P[3*count+2]= Pz;
			count++;
		}
	}

	// now it's ket side data
	Double CD2 = (C[0]-D[0])*(C[0]-D[0])+(C[1]-D[1])*(C[1]-D[1])+(C[2]-D[2])*(C[2]-D[2]);
	Int jnp2 = knp*lnp;
	vector<Double> jexp2(jnp2,ZERO);
	vector<Double> jexpdiff(jnp2,ZERO);
	vector<Double> fket(jnp2,ZERO);
	vector<Double> Q(3*jnp2,ZERO);
	count = 0;
	for(Int lp=0; lp<lnp; lp++) {
		for(Int kp=0; kp<knp; kp++) {

			// prefactors etc.
			Double ia   = kexp[kp];
			Double ja   = lexp[lp];
			Double alpla= ia+ja;
			Double diff = ia-ja;
			Double ab   = -ia*ja/alpla;
			Double pref = exp(ab*CD2)*pow(PI/alpla,1.5E0);
			jexp2[count]= ONE/alpla;
			fket[count] = pref;
			jexpdiff[count]= diff;

			// form P point according to the
			// Gaussian pritimive product theorem
			Double adab = ia/alpla;
			Double bdab = ja/alpla;
			Double Qx   = C[0]*adab + D[0]*bdab;
			Double Qy   = C[1]*adab + D[1]*bdab;
			Double Qz   = C[2]*adab + D[2]*bdab;
			Q[3*count+0]= Qx;
			Q[3*count+1]= Qy;
			Q[3*count+2]= Qz;
			count++;
		}
	}

	/////////////////////////////////////////////////////////////////////////////
	// now it's the formal timing compare
	/////////////////////////////////////////////////////////////////////////////

	// set the angular momentum of the results
	// here the user need to do that
	Int lmin1 = 0;
	Int lmax1 = 1;
	Int lmin2 = 0;
	Int lmax2 = 1;
	Int lmin3 = 0;
	Int lmax3 = 1;
	Int lmin4 = 0;
	Int lmax4 = 1;
	Int nBas1 = ((lmax1+1)*(lmax1+2)*(lmax1+3)-lmin1*(lmin1+1)*(lmin1+2))/6;
	Int nBas2 = ((lmax2+1)*(lmax2+2)*(lmax2+3)-lmin2*(lmin2+1)*(lmin2+2))/6;
	Int nBas3 = ((lmax3+1)*(lmax3+2)*(lmax3+3)-lmin3*(lmin3+1)*(lmin3+2))/6;
	Int nBas4 = ((lmax4+1)*(lmax4+2)*(lmax4+3)-lmin4*(lmin4+1)*(lmin4+2))/6;

	// make the coefficients pair
	// bra side
	Int nL1 = lmax1-lmin1+1;
	Int nL2 = lmax2-lmin2+1;
	count = 0;
	vector<Double> braCoePair(inp*jnp*nL1*nL2,ZERO);
	for(Int j=0; j<nL2; j++) {
		for(Int i=0; i<nL1; i++) {
			for(Int jp=0; jp<jnp; jp++) {
				for(Int ip=0; ip<inp; ip++) {
					Double ic = icoe[ip+i*inp];
					Double jc = jcoe[jp+j*jnp];
					braCoePair[count] = ic*jc;
					count++;
				}
			}
		}
	}

	// ket side
	nL1 = lmax3-lmin3+1;
	nL2 = lmax4-lmin4+1;
	count = 0;
	vector<Double> ketCoePair(knp*lnp*nL1*nL2,ZERO);
	for(Int j=0; j<nL2; j++) {
		for(Int i=0; i<nL1; i++) {
			for(Int jp=0; jp<lnp; jp++) {
				for(Int ip=0; ip<knp; ip++) {
					Double ic = kcoe[ip+i*inp];
					Double jc = lcoe[jp+j*jnp];
					ketCoePair[count] = ic*jc;
					count++;
				}
			}
		}
	}

	// now set up the result vectors
	// N is the total number of running times
	Int N = 1000000;
	Double pMax   = 1.0E0;
	Double omega  = 0.0E0;
	vector<Double> result1(nBas1*nBas2*nBas3*nBas4);
	timer t1;
	for(Int i=0; i<N; i++) {
		hgp_os_eri_sp_sp_sp_sp(inp2,jnp2,pMax,omega,
				&braCoePair.front(),&iexp2.front(),&fbra.front(),&P.front(),A,B,
				&ketCoePair.front(),&jexp2.front(),&fket.front(),&Q.front(),C,D,
				&result1.front());
	}
	printf("hgp sp_sp_sp_sp energy time consuming: %-14.7f\n", t1.elapsed());

	// this is another integral file
	vector<Double> result2(nBas1*nBas2*nBas3*nBas4*9); // dimension is responsible by the user
	timer t2;
	for(Int i=0; i<N; i++) {
		hgp_os_eri_sp_sp_sp_sp_d1(inp2,jnp2,pMax,omega,
				&braCoePair.front(),&iexp2.front(),&iexpdiff.front(),&fbra.front(),&P.front(),A,B,
				&ketCoePair.front(),&jexp2.front(),&jexpdiff.front(),&fket.front(),&Q.front(),C,D,
				&result2.front());
	}
	printf("hgp sp_sp_sp_sp gradient time consuming: %-14.7f\n", t2.elapsed());

	return 0;

}


//*****************************************************************************
// Class: CLaricobiusNigrinus
//
//
// Description: the CLaricobiusNigrinus represents a group of LNF insect. scale by m_ScaleFactor
//*****************************************************************************
// 15/10/2019   Rémi Saint-Amant    Creation
//*****************************************************************************

#include "LaricobiusNigrinusEquations.h"
#include "LaricobiusNigrinus.h"
//#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/logistic.hpp>

using namespace std;
using namespace WBSF::HOURLY_DATA;
using namespace WBSF::LNF;

namespace WBSF
{

	//*********************************************************************************
	//CLaricobiusNigrinus class

	//*****************************************************************************
	// Object creator
	//
	// Input: See CIndividual creator
	//
	// Note: m_RDR (relative Development Rate)  member is init with random values.
	//*****************************************************************************
	CLaricobiusNigrinus::CLaricobiusNigrinus(CHost* pHost, CTRef creationDate, double age, TSex sex, bool bFertil, size_t generation, double scaleFactor) :
		CIndividual(pHost, creationDate, age, sex, bFertil, generation, scaleFactor)
	{
		//reset creation date
		int year = creationDate.GetYear();
		m_creationDate = GetCreationDate(year);
		m_adult_emergence = GetAdultEmergence(year);

		m_adult_longevity = Equations().GetAdultLongevity(m_sex);
		m_F = (m_sex == FEMALE) ? Equations().GetFecondity(m_adult_longevity) : 0;
	}

	CTRef CLaricobiusNigrinus::GetCreationDate(int year)const
	{
		CTRef creationDate;
		double creationCDD = Equations().GetCreationCDD();

		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		CTRef begin = CTRef(year, JANUARY, DAY_01);
		CTRef end = CTRef(year, JUNE, DAY_30);

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !creationDate.IsInit(); TRef++)
		{
			const CWeatherDay& wDay = weather_station.GetDay(TRef);
			double DD = GetStand()->m_DD.GetDD(wDay);
			CDD += DD;
			if (CDD >= creationCDD)
			{
				creationDate = wDay.GetTRef();
			}
		}

		ASSERT(creationDate.IsInit());

		return creationDate;
	}

	CTRef CLaricobiusNigrinus::GetAdultEmergence(int year)const
	{
		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		CTPeriod p = weather_station.GetEntireTPeriod(CTM::DAILY);

		CTRef adult_emergence;
		double adult_emerging_CDD = Equations().GetAdultEmergingCDD();

		CTRef begin = GetStand()->m_diapause_end;
		CTRef end = p.End();
		if(weather_station[year].HaveNext())
			end = min(p.End(), CTRef(begin.GetYear() + 1, JUNE, DAY_30));

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !adult_emergence.IsInit(); TRef++)
		{
			const CWeatherDay& wday = weather_station.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];
			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);

			double DD = max(0.0, T - Equations().m_EAS[Τᴴ]);
			CDD += DD;
			if (CDD >= adult_emerging_CDD)
			{
				adult_emergence = wday.GetTRef();
			}
		}

		return adult_emergence;
	}

	CLaricobiusNigrinus& CLaricobiusNigrinus::operator=(const CLaricobiusNigrinus& in)
	{
		if (&in != this)
		{
			CIndividual::operator=(in);

			m_adult_emergence = in.m_adult_emergence;
			m_adult_longevity = in.m_adult_longevity;// Equations().GetAdultLongevity(m_sex) / 2;
			m_F = in.m_F;
		}

		return *this;
	}

	//destructor
	CLaricobiusNigrinus::~CLaricobiusNigrinus(void)
	{}

	double CLaricobiusNigrinus::AdjustTLab(const string& name, size_t s, CTRef TRef, double T)
	{
		if (name == "BlacksburgLab")
		{
			if (s == -1)
			{
				if (TRef.GetJDay() < CTRef(0, MARCH, DAY_25).GetJDay())
					s = EGG;
				else if (TRef.GetJDay() < CTRef(0, APRIL, DAY_15).GetJDay())
					s = LARVAE;
				else if (TRef.GetJDay() < CTRef(0, MAY, DAY_25).GetJDay())
					s = PUPAE;
				else
					s = AESTIVAL_DIAPAUSE_ADULT;
			}

			//if we are in Blacksburg lab situation, take temperature depend of year
			//lab rearing: change temperature Lamb(2005) thesis
			if (s == LARVAE)
				T = 13;
			else if (s == PREPUPAE || s == PUPAE)
				T = 15;
			else if (s == AESTIVAL_DIAPAUSE_ADULT && TRef.GetYear() == 2003)
				T = 15;
			else if (s == AESTIVAL_DIAPAUSE_ADULT && TRef.GetYear() == 2004)
			{
				if (TRef < CTRef(2004, SEPTEMBER, DAY_27))
					T = 19;
				else
					T = 13;
			}
		}

		return T;
	}

	double CLaricobiusNigrinus::AdjustDLLab(const string& name, size_t s, CTRef TRef, double day_length)
	{
		/*if (name == "VictoriaLab")
		{
			day_length = 12;
		}
		else */
		if (name == "BlacksburgLab")
		{
			//if we are in Blacksburg lab situation, take 12 hours
			if (TRef.GetYear() == 2003)
				day_length = 14;
			else if (TRef < CTRef(2004, SEPTEMBER, DAY_27))
				day_length = 16;
			else
				day_length = 10;
		}

		return day_length;
	}

	void CLaricobiusNigrinus::OnNewDay(const CWeatherDay& weather)
	{
		CIndividual::OnNewDay(weather);

		if (weather.GetTRef() == m_creationDate)
		{
			m_age = EGG;
		}
	}

	//*****************************************************************************
	// Develops all stages for one time step
	// Input:	weather: weather of the hour
	//			timeStep: timeStep [h]
	//*****************************************************************************
	void CLaricobiusNigrinus::Live(const CHourlyData& weather, size_t timeStep)
	{
		assert(IsAlive());
		assert(m_status == HEALTHY);

		CLNFHost* pHost = GetHost();
		CLNFStand* pStand = GetStand();

		double nb_steps = (24.0 / timeStep);
		size_t h = weather.GetTRef().GetHour();
		size_t s = GetStage();

		double T = weather[H_TAIR];
		T = AdjustTLab(weather.GetWeatherStation()->m_name, s, weather.GetTRef(), T);

		double day_length = weather.GetLocation().GetDayLength(weather.GetTRef()) / 3600.0;//[h]
		day_length = AdjustDLLab(weather.GetWeatherStation()->m_name, s, weather.GetTRef(), day_length);

		if (s < AESTIVAL_DIAPAUSE_ADULT)
		{
			//Time step development rate
			double r = Equations().GetRate(s, T) / nb_steps;

			double corr_r = (s == EGG || s == LARVAE) ? Equations().m_RDR[s][0] : 1;

			//Relative development rate for this individual
			//double rr = (s == EGG || s == LARVAE) ? m_RDR[s] : 1;

			//Time step development rate for this individual
			r *= corr_r;
			ASSERT(r >= 0 && r < 1);

			//Adjust age
			m_age += r;

			if (!m_dropToGroundDate.IsInit() && m_age > LARVAE + 0.9)//drop to the soil when 90% competed (guess)
				m_dropToGroundDate = weather.GetTRef().as(CTM::DAILY);
		}
		else if (s == AESTIVAL_DIAPAUSE_ADULT)
		{
			CTRef TRef = weather.GetTRef().as(CTM::DAILY);
			if (TRef == m_adult_emergence)
				m_age = ACTIVE_ADULT;
		}
		else//ACTIVE_ADULT
		{
			double r = (1.0 / m_adult_longevity) / nb_steps;
			ASSERT(r >= 0 && r < 1);

			m_age += r;
		}
	}

	//*****************************************************************************
	// Develops all stages, including adults
	// Input:	weather: the weather of the day
	//*****************************************************************************
	void CLaricobiusNigrinus::Live(const CWeatherDay& weather)
	{
		CIndividual::Live(weather);

		ASSERT(IsCreated(weather.GetTRef()));

		if (!IsCreated(weather.GetTRef()))
			return;

		size_t nbSteps = GetTimeStep().NbSteps();
		for (size_t step = 0; step < nbSteps&&m_age < DEAD_ADULT; step++)
		{
			size_t h = step * GetTimeStep();
			Live(weather[h], GetTimeStep());
		}

		if (weather.GetTRef() == m_creationDate || HasChangedStage())
			m_reachDate[GetStage()] = weather.GetTRef();
	}

	void CLaricobiusNigrinus::Brood(const CWeatherDay& weather)
	{
		assert(IsAlive() && m_sex == FEMALE);

		if (GetStage() == ACTIVE_ADULT)
		{
			//no brood process done

			//brooding
			//m_broods = m_F;
			//m_totalBroods = m_F;
			//m_F = 0;
		}
	}

	// kills by old age and frost
	// Output:  Individual's state is updated to follow update
	void CLaricobiusNigrinus::Die(const CWeatherDay& weather)
	{
		//attrition mortality. Killed at the end of time step
		if (GetStage() == DEAD_ADULT)
		{
			//Old age
			m_status = DEAD;
			m_death = OLD_AGE;
		}
		else
		{
			size_t s = GetStage();

			//Preliminary assessment of the cold tolerance of Laricobius nigrinus, a winter - active predator of the hemlock woolly adelgid from western canada
			//Leland M.Humble
			static const double COLD_TOLERENCE_T[NB_STAGES] = { -27.5,-22.1, -99.0,-99.0,-19.0,-19.0 };
			//Toland:L. nigrinus was -13.6 oC (± 0.5) with temperatures that ranged from -6 oC to -21 oC.
			if (weather[H_TMIN][MEAN] < COLD_TOLERENCE_T[s])
			{
				m_status = DEAD;
				m_death = FROZEN;
			}
		}

	}

	//*****************************************************************************
	// GetStat gather information of this object
	//
	// Input: stat: the statistic object
	// Output: The stat is modified
	//*****************************************************************************
	void CLaricobiusNigrinus::GetStat(CTRef d, CModelStat& stat)
	{
		if (IsCreated(d))
		{
			size_t s = GetStage();
			ASSERT(s <= DEAD_ADULT);

			if (IsAlive() || (s == DEAD_ADULT))
				stat[S_EGG + s] += m_scaleFactor;

			if (m_status == DEAD && m_death == FROZEN)
				stat[S_DEAD_FROST] += m_scaleFactor;

			if (HasChangedStage())
				stat[S_M_EGG + s] += m_scaleFactor;

			//if (s == ACTIVE_ADULT)
			//{
			//	stat[S_ADULT_ABUNDANCE] += m_scaleFactor * m_adult_abundance;
			//}
		}
	}

	void CLaricobiusNigrinus::Pack(const CIndividualPtr& pBug)
	{
		assert(m_sex == pBug->GetSex());

		CLaricobiusNigrinus* in = (CLaricobiusNigrinus*)(pBug.get());
		CIndividual::Pack(pBug);
	}

	double CLaricobiusNigrinus::GetInstar(bool includeLast)const
	{
		return (IsAlive() || m_death == OLD_AGE) ? GetStage() : CBioSIMModelBase::VMISS;
	}

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

	//*********************************************************************************
	//CLNFHost

	CLNFHost::CLNFHost(CStand* pStand) :
		CHost(pStand)
	{
	}

	void CLNFHost::Live(const CWeatherDay& weather)
	{
		CHost::Live(weather);
	}

	void CLNFHost::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CHost::GetStat(d, stat, generation);
	}

	//*************************************************
	//CLNFStand

	void CLNFStand::init(int year, const CWeatherYears& weather)
	{
		m_diapause_end = ComputeDiapauseEnd(weather[year]);
	}

	CTRef CLNFStand::ComputeDiapauseEnd(const CWeatherYear& weather)const
	{
		CTPeriod p = weather.GetEntireTPeriod(CTM::DAILY);

		double sumDD = 0;

		for (size_t ii = (m_equations.m_ADE[ʎ0] - 1); ii <= (m_equations.m_ADE[ʎ1] - 1); ii++)
		{
			CTRef TRef = p.Begin() + ii;
			const CWeatherDay& wday = weather.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];

			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);
			double DD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative
			sumDD += DD;
		}

		boost::math::logistic_distribution<double> begin_dist(m_equations.m_ADE[ʎ2], m_equations.m_ADE[ʎ3]);
		int begin = (int)Round((m_equations.m_ADE[ʎ1] - 1) + m_equations.m_ADE[ʎa] * cdf(begin_dist, sumDD), 0);

		return p.Begin() + begin;
	}

	void CLNFStand::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CStand::GetStat(d, stat, generation);

		const CWeatherStation& weather_station = GetModel()->m_weather;
		const CWeatherDay& wday = weather_station.GetDay(d);

		//use year of diapause to compute correctly the adult emergence cdd
		int year = m_diapause_end.GetYear();
		CTRef begin = CTRef(year, JANUARY, DAY_01);
		CTRef end = CTRef(year, DECEMBER, DAY_31);

		if (d >= begin && d<= end)
		{
			//Egg creation DD (allen 1976)
			m_egg_creation_CDD += m_DD.GetDD(wday);
			stat[S_EGG_CREATION_CDD] = m_egg_creation_CDD;

			//diapause end negative DD
			double T = wday[H_TNTX][MEAN];
			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);
			//T = max(m_equations.m_ADE[ʎa], T);
			double NDD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative

			int ii = d-begin;
			//if( ii>=(172-1) && ii<=int(m_equations.m_ADE[ʎ0]-1))
			if (ii >= int(m_equations.m_ADE[ʎ0] - 1) && ii <= int(m_equations.m_ADE[ʎ1] - 1))
				m_diapause_end_NCDD += NDD;

			stat[S_DIAPAUSE_END_NCDD] = m_diapause_end_NCDD;
		}

		begin = m_diapause_end;
		end = CTRef(m_diapause_end.GetYear()+1, JANUARY, DAY_31);
		if (d >= begin && d <= end)
		{
			//adult emergence (growing DD)
			double T = wday[H_TNTX][MEAN];
			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);

			double GDD = max(0.0, T - m_equations.m_EAS[Τᴴ]);
			m_adult_emergence_CDD += GDD;
			stat[S_ADULT_EMERGENCE_CDD] = m_adult_emergence_CDD;
		}
	}

}


#include "CalibTracker/SiStripLorentzAngle/interface/LA_Filler_Fitter.h"
#include "FWCore/Utilities/interface/EDMException.h"

#include <cmath>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/erase.hpp>
#include <TF1.h>

void LA_Filler_Fitter::
fit_width_profile(Book& book) {
  for(Book::iterator it = book.begin(".*"+method(WIDTH)); it!=book.end(); ++it) {
    it->second->SetTitle("Mean Cluster Width;tan#theta_{t}");
    TH1* const p = it->second;
    if(p->GetEntries() < 400) { delete p; book[it->first]=nullptr; continue;}
    p->SetTitle(";tan#theta_{t};");
    const float min = p->GetMinimum();
    const float max = p->GetMaximum();
    float xofmin = p->GetBinCenter(p->GetMinimumBin()); if( xofmin>0.0 || xofmin<-0.15) xofmin = -0.05;
    const float xofmax = p->GetBinCenter(p->GetMaximumBin());

    TF1* const fit = new TF1("LA_profile_fit","[2]*(TMath::Abs(x-[0]))+[1]",-1,1);
    fit->SetParLimits(0,-0.15,0.01);
    fit->SetParLimits(1, 0.6*min, 1.25*min );
    fit->SetParLimits(2,0.1,10);
    fit->SetParameters( xofmin, min, (max-min) / fabs( xofmax - xofmin ) );

    int badfit = p->Fit(fit,"IEQ","",-.5,.3);
    if( badfit ) badfit = p->Fit(fit,"IEQ","", -.46,.26);
    if( badfit ) { book.erase(it); }
  }
}

void LA_Filler_Fitter::
make_and_fit_symmchi2(Book& book) {
  for(Book::iterator it = book.begin(".*_all"); it!=book.end(); ++it) {
    const std::string base = boost::erase_all_copy(it->first,"_all");

    std::vector<Book::iterator> rebin_hists;
    const Book::iterator&    all = it;	                             rebin_hists.push_back(all);
    Book::iterator     w1 = book.find(base+"_w1");           rebin_hists.push_back(w1);
    Book::iterator var_w2 = book.find(base+method(AVGV2,false)); rebin_hists.push_back(var_w2);
    Book::iterator var_w3 = book.find(base+method(AVGV3,false)); rebin_hists.push_back(var_w3);

    const unsigned rebin = std::max( var_w2==book.end() ? 0 : find_rebin(var_w2->second),
				     var_w3==book.end() ? 0 : find_rebin(var_w3->second) );
    BOOST_FOREACH(Book::iterator it, rebin_hists) if(it!=book.end()) it->second->Rebin( rebin>1 ? rebin<7 ? rebin : 6 : 1);

    TH1* const prob_w1 = w1==book.end()     ? nullptr : subset_probability( base+method(PROB1,false) ,w1->second,all->second);
    TH1* const rmsv_w2 = var_w2==book.end() ? nullptr :        rms_profile( base+method(RMSV2,false), (TProfile*const)var_w2->second);
    TH1* const rmsv_w3 = var_w3==book.end() ? nullptr :        rms_profile( base+method(RMSV3,false), (TProfile*const)var_w3->second);

    std::vector<TH1*> fit_hists;
    if(prob_w1) {
      book.book(base+method(PROB1,false),prob_w1);
      fit_hists.push_back(prob_w1);  prob_w1->SetTitle("Width==1 Probability;tan#theta_{t}-(dx/dz)_{reco}");
    }
    if(var_w2!=book.end())  {
      book.book(base+method(RMSV2,false),rmsv_w2);
      fit_hists.push_back(var_w2->second);   var_w2->second->SetTitle("Width==2 Mean Variance;tan#theta_{t}-(dx/dz)_{reco}");
      fit_hists.push_back(rmsv_w2);                 rmsv_w2->SetTitle("Width==2 RMS Variance;tan#theta_{t}-(dx/dz)_{reco}");
    }
    if(var_w3!=book.end())  {
      book.book(base+method(RMSV3,false),rmsv_w3);
      fit_hists.push_back(var_w3->second);   var_w3->second->SetTitle("Width==3 Mean Variance;tan#theta_{t}-(dx/dz)_{reco}");
      fit_hists.push_back(rmsv_w3);                 rmsv_w3->SetTitle("Width==3 RMS Variance;tan#theta_{t}-(dx/dz)_{reco}");
    }

    if(fit_hists.empty()) continue;
    const unsigned bins = fit_hists[0]->GetNbinsX();
    const unsigned guess = fit_hists[0]->FindBin(0);
    const std::pair<unsigned,unsigned> range(guess-bins/30,guess+bins/30);

    BOOST_FOREACH(TH1*const hist, fit_hists) {
      TH1*const chi2 = SymmetryFit::symmetryChi2(hist,range);
      if(chi2) {book.book(chi2->GetName(),chi2); chi2->SetTitle("Symmetry #chi^{2};tan#theta_{t}-(dx/dz)_{reco}");}
    }
  }
}

unsigned LA_Filler_Fitter::
find_rebin(const TH1* const hist) {
  const double mean = hist->GetMean();
  const double rms = hist->GetRMS();
  const int begin = std::min(                1, hist->GetXaxis()->FindFixBin(mean-rms));
  const int end   = std::max(hist->GetNbinsX(), hist->GetXaxis()->FindFixBin(mean+rms)) + 1;
  unsigned current_hole(0), max_hole(0);
  for(int i=begin; i<end; i++) {
    if(!hist->GetBinError(i)) current_hole++;
    else if(current_hole) {max_hole = std::max(current_hole,max_hole); current_hole=0;}
  }
  return max_hole+1;
}

TH1* LA_Filler_Fitter::
rms_profile(const std::string name, const TProfile* const prof) {
  const int bins = prof->GetNbinsX();
  TH1* const rms = new TH1F(name.c_str(),"",bins, prof->GetBinLowEdge(1),  prof->GetBinLowEdge(bins) + prof->GetBinWidth(bins) );
  for(int i = 1; i<=bins; i++) {
    const double Me = prof->GetBinError(i);
    const double neff = prof->GetBinEntries(i); //Should be prof->GetBinEffectiveEntries(i);, not availible this version ROOT.  This is only ok for unweighted fills
    rms->SetBinContent(i, Me*sqrt(neff) );
    rms->SetBinError(i, Me/sqrt(2) );
  }
  return rms;
}

TH1* LA_Filler_Fitter::
subset_probability(const std::string name, const TH1* const subset, const TH1* const total) {
  const int bins = subset->GetNbinsX();
  TH1* const prob = new TH1F(name.c_str(),"",bins, subset->GetBinLowEdge(1),  subset->GetBinLowEdge(bins) + subset->GetBinWidth(bins) );
  for(int i = 1; i<=bins; i++) {
    const double s = subset->GetBinContent(i);
    const double T = total->GetBinContent(i);
    const double B = T-s;

    const double p = T? s/T : 0;
    const double perr = T? ( (s&&B)? sqrt(s*s*B+B*B*s)/(T*T) : 1/T ) : 0;

    prob->SetBinContent(i,p);
    prob->SetBinError(i,perr);
  }
  return prob;
}


// Copyright 2004 The Trustees of Indiana University.

// Use, modification and distribution is subject to 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_DIJKSTRA_TESTING_DIETMAR
#define BOOST_GRAPH_DIJKSTRA_TESTING
#endif

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/timer/timer.hpp>
#include <vector>
#include <iostream>

#include <iterator>
#include <utility>
#include <boost/random/uniform_int.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/detail/lightweight_test.hpp>

using namespace boost;

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING_DIETMAR

struct show_events_visitor : dijkstra_visitor<>
{
    template < typename Vertex, typename Graph >
    void discover_vertex(Vertex v, const Graph&)
    {
        std::cerr << "on_discover_vertex(" << v << ")\n";
    }

    template < typename Vertex, typename Graph >
    void examine_vertex(Vertex v, const Graph&)
    {
        std::cerr << "on_discover_vertex(" << v << ")\n";
    }
};

template < typename Graph, typename Kind >
void run_test(const Graph& g, const char* name, Kind kind,
    const std::vector< double >& correct_distances)
{
    std::vector< double > distances(num_vertices(g));

    std::cout << "Running Dijkstra's with " << name << "...";
    std::cout.flush();
    boost::timer::cpu_timer t;
    t.start();
    dijkstra_heap_kind = kind;

    dijkstra_shortest_paths(g, vertex(0, g),
        distance_map(&distances[0]).visitor(show_events_visitor()));
    double run_time = t.stop();
    std::cout << t.format() << " seconds.\n";

    BOOST_TEST(distances == correct_distances);

    if (distances != correct_distances)
    {
        std::cout << "Expected: ";
        std::copy(correct_distances.begin(), correct_distances.end(),
            std::ostream_iterator< double >(std::cout, " "));
        std::cout << "\nReceived: ";
        std::copy(distances.begin(), distances.end(),
            std::ostream_iterator< double >(std::cout, " "));
        std::cout << std::endl;
    }
}
#endif

int main(int argc, char* argv[])
{
    unsigned n = (argc > 1 ? lexical_cast< unsigned >(argv[1]) : 10000u);
    unsigned m = (argc > 2 ? lexical_cast< unsigned >(argv[2]) : 10 * n);
    int seed = (argc > 3 ? lexical_cast< int >(argv[3]) : 1);

    // Build random graph
    typedef adjacency_list< vecS, vecS, directedS, no_property,
        property< edge_weight_t, double > >
        Graph;
    std::cout << "Generating graph...";
    std::cout.flush();
    minstd_rand gen(seed);
    double p = double(m) / (double(n) * double(n));
    Graph g(erdos_renyi_iterator< minstd_rand, Graph >(gen, n, p),
        erdos_renyi_iterator< minstd_rand, Graph >(), n);
    std::cout << n << " vertices, " << num_edges(g) << " edges.\n";
    uniform_real< double > rand01(0.0, 1.0);
    graph_traits< Graph >::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(edge_weight, g, *ei, rand01(gen));

    std::vector< double > binary_heap_distances(n);
    std::vector< double > relaxed_heap_distances(n);
    std::vector< double > no_color_map_distances(n);

    // Run binary or d-ary heap version
    std::cout << "Running Dijkstra's with binary heap...";
    std::cout.flush();
    boost::timer::cpu_timer t;
    t.start();

    dijkstra_shortest_paths(g, vertex(0, g),
        distance_map(boost::make_iterator_property_map(
            binary_heap_distances.begin(), get(boost::vertex_index, g))));
    t.stop();
    boost::timer::cpu_times binary_heap_time = t.elapsed();
    std::cout << boost::timer::format(binary_heap_time) << " seconds.\n";

    std::cout << "Running Dijkstra's with d-ary heap (d=4)...";
    std::cout.flush();
    t.start();

    dijkstra_shortest_paths(g, vertex(0, g),
        distance_map(boost::make_iterator_property_map(
            relaxed_heap_distances.begin(), get(boost::vertex_index, g))));
    boost::timer::cpu_times relaxed_heap_time = t.elapsed();
    std::cout << boost::timer::format(relaxed_heap_time) << " seconds.\n"
              << "Speedup = " << (binary_heap_time.user / relaxed_heap_time.user)
              << ".\n";

    // Verify that the results are equivalent
    BOOST_TEST(binary_heap_distances == relaxed_heap_distances);

    // Run Michael's no-color-map version

    std::cout << "Running Dijkstra's (no color map) with d-ary heap (d=4)...";
    std::cout.flush();

    t.start();
    dijkstra_shortest_paths_no_color_map(g, vertex(0, g),
        boost::dummy_property_map(),
        boost::make_iterator_property_map(
            no_color_map_distances.begin(), get(boost::vertex_index, g), 0.),
        get(boost::edge_weight, g), get(boost::vertex_index, g),
        std::less< double >(), boost::closed_plus< double >(),
        (std::numeric_limits< double >::max)(), 0,
        make_dijkstra_visitor(null_visitor()));
    boost::timer::cpu_times no_color_map_time = t.elapsed();
    std::cout << boost::timer::format(no_color_map_time) << " seconds.\n"
              << "Speedup = " << (binary_heap_time.user / no_color_map_time.user)
              << ".\n";

    // Verify that the results are equivalent
    BOOST_TEST(binary_heap_distances == no_color_map_distances);

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING_DIETMAR
    run_test(g, "d-ary heap (d=2)", dijkstra_d_heap_2, binary_heap_distances);
    run_test(g, "d-ary heap (d=3)", dijkstra_d_heap_3, binary_heap_distances);
    run_test(
        g, "Fibonacci heap", dijkstra_fibonacci_heap, binary_heap_distances);
    run_test(g, "Lazy Fibonacci heap", dijkstra_lazy_fibonacci_heap,
        binary_heap_distances);
    run_test(g, "Pairing heap", dijkstra_pairing_heap, binary_heap_distances);
    run_test(g, "Splay heap", dijkstra_splay_heap, binary_heap_distances);
#endif
    return boost::report_errors();
}


// Copyright John Maddock 2015.

// Use, modification and distribution are subject to 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)

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <nil/crypto3/multiprecision/cpp_bin_float.hpp>
#include <nil/crypto3/multiprecision/cpp_dec_float.hpp>
#include <nil/crypto3/multiprecision/debug_adaptor.hpp>
#include <nil/crypto3/multiprecision/logged_adaptor.hpp>

#ifdef TEST_FLOAT128
#include <nil/crypto3/multiprecision/float128.hpp>
#endif
#ifdef TEST_GMP
#include <nil/crypto3/multiprecision/gmp.hpp>
#endif
#ifdef TEST_MPFR
#include <nil/crypto3/multiprecision/mpfr.hpp>
#endif
#ifdef TEST_MPFI
#include <nil/crypto3/multiprecision/mpfi.hpp>
#endif
#ifdef TEST_TOMMATH
#include <nil/crypto3/multiprecision/tommath.hpp>
#endif

#include <boost/functional/hash.hpp>

#include "test.hpp"
#include <iostream>
#include <iomanip>

template<class T>
void test() {
    T val = 23;
    std::size_t t1 = boost::hash<T>()(val);
    BOOST_CHECK(t1);

#ifndef BOOST_NO_CXX11_HDR_FUNCTIONAL
    std::size_t t2 = std::hash<T>()(val);
    BOOST_CHECK_EQUAL(t1, t2);
#endif
    val = -23;
    std::size_t t3 = boost::hash<T>()(val);
    BOOST_CHECK_NE(t1, t3);
#ifndef BOOST_NO_CXX11_HDR_FUNCTIONAL
    t2 = std::hash<T>()(val);
    BOOST_CHECK_EQUAL(t3, t2);
#endif
}

int main() {
    test<nil::crypto3::multiprecision::cpp_int>();
    test<nil::crypto3::multiprecision::checked_int1024_t>();
    // test<nil::crypto3::multiprecision::checked_uint512_t >();
    test<nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::cpp_int_backend<
        64, 64, nil::crypto3::multiprecision::signed_magnitude, nil::crypto3::multiprecision::checked, void>>>();

    test<nil::crypto3::multiprecision::cpp_bin_float_100>();
    test<nil::crypto3::multiprecision::cpp_dec_float_100>();

    test<nil::crypto3::multiprecision::cpp_rational>();

    test<nil::crypto3::multiprecision::number<
        nil::crypto3::multiprecision::debug_adaptor<nil::crypto3::multiprecision::cpp_int::backend_type>>>();

    test<nil::crypto3::multiprecision::number<
        nil::crypto3::multiprecision::logged_adaptor<nil::crypto3::multiprecision::cpp_int::backend_type>>>();

#ifdef TEST_FLOAT128
    test<nil::crypto3::multiprecision::float128>();
#endif
#ifdef TEST_GMP
    test<nil::crypto3::multiprecision::mpz_int>();
    test<nil::crypto3::multiprecision::mpq_rational>();
    test<nil::crypto3::multiprecision::mpf_float>();
#endif

#ifdef TEST_MPFR
    test<nil::crypto3::multiprecision::mpfr_float_50>();
#endif
#ifdef TEST_MPFI
    test<nil::crypto3::multiprecision::mpfi_float_50>();
#endif

#ifdef TEST_TOMMATH
    test<nil::crypto3::multiprecision::tom_int>();
    test<nil::crypto3::multiprecision::tom_rational>();
#endif

    return boost::report_errors();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.
// Copyright (c) 2013-2014 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_POINT_TO_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_POINT_TO_GEOMETRY_HPP

#include <utility>

#include <boost/range.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/interior_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/distance_comparable_to_regular.hpp>

#include <boost/geometry/views/closeable_view.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

#include <boost/geometry/algorithms/dispatch/distance.hpp>

#include <boost/geometry/algorithms/detail/distance/default_strategies.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

template <typename P1, typename P2, typename Strategy>
struct point_to_point
{
    static inline
    typename strategy::distance::services::return_type<Strategy, P1, P2>::type
    apply(P1 const& p1, P2 const& p2, Strategy const& strategy)
    {
        boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(p1, p2);
    }
};

template
<
    typename Point,
    typename Range,
    closure_selector Closure,
    typename Strategy
>
class point_to_range
{
private:
    typedef typename strategy::distance::services::comparable_type
        <
            Strategy
        >::type comparable_strategy;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy,
            Point,
            typename boost::range_value<Range>::type
        >::type comparable_return_type;

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy,
            Point,
            typename boost::range_value<Range>::type
        >::type return_type;

    static inline return_type apply(Point const& point, Range const& range,
                                    Strategy const& strategy)
    {
        comparable_strategy c_strategy =
            strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        comparable_return_type const zero = comparable_return_type(0);

        if (boost::size(range) == 0)
        {
            return zero;
        }

        typedef typename closeable_view<Range const, Closure>::type view_type;

        view_type view(range);

        // line of one point: return point distance
        typedef typename boost::range_iterator<view_type const>::type iterator_type;
        iterator_type it = boost::begin(view);
        iterator_type prev = it++;
        if (it == boost::end(view))
        {
            return strategy::distance::services::comparable_to_regular
            <
                comparable_strategy, Strategy, Point,
                typename boost::range_value<Range>::type
            >::apply( c_strategy.apply(point,
                                       *boost::begin(view),
                                       *boost::begin(view)) );
        }

        // start with first segment distance
        comparable_return_type cd = c_strategy.apply(point, *prev, *it);

        // check if other segments are closer
        for (++prev, ++it; it != boost::end(view); ++prev, ++it)
        {
            comparable_return_type cds = c_strategy.apply(point, *prev, *it);
            if (geometry::math::equals(cds, zero))
            {
                return strategy::distance::services::comparable_to_regular
                    <
                        comparable_strategy,
                        Strategy,
                        Point,
                        typename boost::range_value<Range>::type
                    >::apply(zero);
            }
            else if (cds < cd)
            {
                cd = cds;
            }
        }

        return strategy::distance::services::comparable_to_regular
            <
                comparable_strategy,
                Strategy,
                Point,
                typename boost::range_value<Range>::type
            >::apply(cd);
    }
};

template
<
    typename Point,
    typename Ring,
    closure_selector Closure,
    typename Strategy
>
struct point_to_ring
{
    typedef std::pair
        <
            typename strategy::distance::services::return_type
                <
                    Strategy, Point, typename point_type<Ring>::type
                >::type,
            bool
        > distance_containment;

    static inline distance_containment apply(Point const& point,
                                             Ring const& ring,
                                             Strategy const& strategy)
    {
        return distance_containment
            (
                point_to_range
                    <
                        Point,
                        Ring,
                        Closure,
                        Strategy
                    >::apply(point, ring, strategy),
                geometry::within(point, ring)
            );
    }
};

template
<
    typename Point,
    typename Polygon,
    closure_selector Closure,
    typename Strategy
>
class point_to_polygon
{
private:
    typedef typename strategy::distance::services::comparable_type
        <
            Strategy
        >::type comparable_strategy;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy, Point, typename point_type<Polygon>::type
        >::type comparable_return_type;

    typedef std::pair
        <
            comparable_return_type, bool
        > comparable_distance_containment;

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy, Point, typename point_type<Polygon>::type
        >::type return_type;
    typedef std::pair<return_type, bool> distance_containment;

    static inline distance_containment apply(Point const& point,
                                             Polygon const& polygon,
                                             Strategy const& strategy)
    {
        comparable_strategy c_strategy =
            strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        // Check distance to all rings
        typedef point_to_ring
            <
                Point,
                typename ring_type<Polygon>::type,
                Closure,
                comparable_strategy
            > per_ring;

        comparable_distance_containment dc =
            per_ring::apply(point, exterior_ring(polygon), c_strategy);

        typename interior_return_type<Polygon const>::type rings
                    = interior_rings(polygon);
        for (typename boost::range_iterator
                 <
                     typename interior_type<Polygon const>::type const
                 >::type it = boost::begin(rings);
             it != boost::end(rings); ++it)
        {
            comparable_distance_containment dcr =
                per_ring::apply(point, *it, c_strategy);
            if (dcr.first < dc.first)
            {
                dc.first = dcr.first;
            }
            // If it was inside, and also inside inner ring,
            // turn off the inside-flag, it is outside the polygon
            if (dc.second && dcr.second)
            {
                dc.second = false;
            }
        }

        return_type rd = strategy::distance::services::comparable_to_regular
            <
                comparable_strategy, Strategy, Point, Polygon
            >::apply(dc.first);

        return std::make_pair(rd, dc.second);
    }
};

}} // namespace detail::distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Point-point
template <typename P1, typename P2, typename Strategy>
struct distance
    <
        P1, P2, Strategy,
        point_tag, point_tag, strategy_tag_distance_point_point,
        false
    >
    : detail::distance::point_to_point<P1, P2, Strategy>
{};

// Point-line version 2, where point-segment strategy is specified
template <typename Point, typename Linestring, typename Strategy>
struct distance
<
    Point, Linestring, Strategy,
    point_tag, linestring_tag, strategy_tag_distance_point_segment,
    false
> : detail::distance::point_to_range<Point, Linestring, closed, Strategy>
{};

// Point-ring , where point-segment strategy is specified
template <typename Point, typename Ring, typename Strategy>
struct distance
<
    Point, Ring, Strategy,
    point_tag, ring_tag, strategy_tag_distance_point_segment,
    false
>
{
    typedef typename strategy::distance::services::return_type
        <
            Strategy, Point, typename point_type<Ring>::type
        >::type return_type;

    static inline return_type apply(Point const& point,
                                    Ring const& ring,
                                    Strategy const& strategy)
    {
        std::pair<return_type, bool>
            dc = detail::distance::point_to_ring
            <
                Point, Ring,
                geometry::closure<Ring>::value,
                Strategy
            >::apply(point, ring, strategy);

        return dc.second ? return_type(0) : dc.first;
    }
};

// Point-polygon , where point-segment strategy is specified
template <typename Point, typename Polygon, typename Strategy>
struct distance
<
    Point, Polygon, Strategy, point_tag, polygon_tag,
    strategy_tag_distance_point_segment, false
>
{
    typedef typename strategy::distance::services::return_type
        <
            Strategy, Point, typename point_type<Polygon>::type
        >::type return_type;

    static inline return_type apply(Point const& point,
                                    Polygon const& polygon,
                                    Strategy const& strategy)
    {
        std::pair<return_type, bool>
            dc = detail::distance::point_to_polygon
            <
                Point, Polygon,
                geometry::closure<Polygon>::value,
                Strategy
            >::apply(point, polygon, strategy);

        return dc.second ? return_type(0) : dc.first;
    }
};

// Point-segment version 2, with point-segment strategy
template <typename Point, typename Segment, typename Strategy>
struct distance
<
    Point, Segment, Strategy,
    point_tag, segment_tag, strategy_tag_distance_point_segment,
    false
>
{
    static inline typename return_type<Strategy, Point, typename point_type<Segment>::type>::type
    apply(Point const& point,
          Segment const& segment,
          Strategy const& strategy)
    {
        typename point_type<Segment>::type p[2];
        geometry::detail::assign_point_from_index<0>(segment, p[0]);
        geometry::detail::assign_point_from_index<1>(segment, p[1]);

        boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(point, p[0], p[1]);
    }
};

template <typename Point, typename Box, typename Strategy>
struct distance
    <
         Point, Box, Strategy, point_tag, box_tag,
         strategy_tag_distance_point_box, false
    >
{
    static inline typename strategy::distance::services::return_type
        <
            Strategy, Point, typename point_type<Box>::type
        >::type
    apply(Point const& point, Box const& box, Strategy const& strategy)
    {
        boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(point, box);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_POINT_TO_GEOMETRY_HPP


//
// Created by kevin on 3/6/17.
//

#ifndef CAFFE_BASE_CONV_LAYER_HPP
#define CAFFE_BASE_CONV_LAYER_HPP
#include <string>
#include <utility>
#include <vector>

#include <boost/unordered_map.hpp>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"
namespace caffe{

	template <typename Dtype>
	class BaseConvolutionLayer : public Layer<Dtype> {
	public:
		explicit BaseConvolutionLayer(const LayerParameter& param)
				: Layer<Dtype>(param) {}
		virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
		const vector<Blob<Dtype>*>& top);
		virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
		const vector<Blob<Dtype>*>& top);

		virtual inline int MinBottomBlobs() const { return 1; }
		virtual inline int MinTopBlobs() const { return 1; }
		virtual inline bool EqualNumBottomTopBlobs() const { return true; }

	protected:
		// Helper functions that abstract away the column buffer and gemm arguments.
		// The last argument in forward_cpu_gemm is so that we can skip the im2col if
		// we just called weight_cpu_gemm with the same input.
		void forward_cpu_gemm(const Dtype* input, const Dtype* weights,
							  Dtype* output, bool skip_im2col = false);
		void forward_cpu_bias(Dtype* output, const Dtype* bias);
		void backward_cpu_gemm(const Dtype* input, const Dtype* weights,
							   Dtype* output);
		void weight_cpu_gemm(const Dtype* input, const Dtype* output, Dtype*
		weights);
		void backward_cpu_bias(Dtype* bias, const Dtype* input);

#ifndef CPU_ONLY
		void forward_gpu_gemm(const Dtype* col_input, const Dtype* weights,
							  Dtype* output, bool skip_im2col = false);
		void forward_gpu_bias(Dtype* output, const Dtype* bias);
		void backward_gpu_gemm(const Dtype* input, const Dtype* weights,
							   Dtype* col_output);
		void weight_gpu_gemm(const Dtype* col_input, const Dtype* output, Dtype*
		weights);
		void backward_gpu_bias(Dtype* bias, const Dtype* input);
#endif
		inline int input_shape(int i) {
			return (*bottom_shape_)[channel_axis_ + i];
		}

		// reverse_dimensions should return true iff we are implementing deconv, so
		// that conv helpers know which dimensions are which.
		virtual bool reverse_dimensions() = 0;
		// Compute height_out_ and width_out_ from other parameters.
		virtual void compute_output_shape() = 0;

		int kernel_h_, kernel_w_;
		int stride_h_, stride_w_;
		int dilation_h_, dilation_w_;
		int num_;
		int channels_;
		int channel_axis_, num_spatial_axes_;
		int bottom_dim_;
		int top_dim_;
		int pad_h_, pad_w_;
		int height_, width_;
		int group_;
		int num_output_;
		int height_out_, width_out_;
		bool bias_term_;
		bool is_1x1_;
		const vector<int>* bottom_shape_;
		vector<int> output_shape_;

	private:
		// wrap im2col/col2im so we don't have to remember the (long) argument lists
		inline void conv_im2col_cpu(const Dtype* data, Dtype* col_buff) {
			im2col_cpu(data, conv_in_channels_, conv_in_height_, conv_in_width_,
					   kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, dilation_h_, dilation_w_, col_buff);
		}
		inline void conv_col2im_cpu(const Dtype* col_buff, Dtype* data) {
			col2im_cpu(col_buff, conv_in_channels_, conv_in_height_, conv_in_width_,
					   kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, dilation_h_, dilation_w_, data);
		}
#ifndef CPU_ONLY
		inline void conv_im2col_gpu(const Dtype* data, Dtype* col_buff) {
			im2col_gpu(data, conv_in_channels_, conv_in_height_, conv_in_width_,
					   kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, dilation_h_, dilation_w_, col_buff);
		}
		inline void conv_col2im_gpu(const Dtype* col_buff, Dtype* data) {
			col2im_gpu(col_buff, conv_in_channels_, conv_in_height_, conv_in_width_,
					   kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, dilation_h_, dilation_w_, data);
		}
#endif

		int conv_out_channels_;
		int conv_in_channels_;
		int conv_out_spatial_dim_;
		int conv_in_height_;
		int conv_in_width_;
		int kernel_dim_;
		int weight_offset_;
		int col_offset_;
		int output_offset_;

		Blob<Dtype> col_buffer_;
		Blob<Dtype> bias_multiplier_;
	};
} // namespace caffe

#endif //CAFFE_BASE_CONV_LAYER_HPP