//
// Created by Lubos Ilcik on 30/03/2018.
//

#include <boost/test/unit_test.hpp>
#include <graphene/chain/vesting_balance_object.hpp>
using namespace graphene::chain;

BOOST_AUTO_TEST_SUITE(coin_days_vesting_balance_tests)

struct vesting_balance_fixture {
   vesting_balance_object vesting_balance;
   const asset initial_balance;
   const fc::time_point_sec beginning;
   const uint32_t vesting_seconds;

   vesting_balance_fixture() : vesting_balance(), initial_balance(100), beginning(), vesting_seconds(100) {
      vesting_balance.owner = account_id_type();
      vesting_balance.balance = initial_balance;
      cdd_vesting_policy policy;
      policy.vesting_seconds = vesting_seconds;
      policy.coin_seconds_earned = 0;
      policy.coin_seconds_earned_last_update = beginning;
      vesting_balance.policy = policy;
   }
};

BOOST_FIXTURE_TEST_CASE(when_vesting_seconds_passed_withdraw_full_balance_is_allowed, vesting_balance_fixture)
{
   BOOST_CHECK(!vesting_balance.is_withdraw_allowed(beginning, initial_balance));
   for (uint32_t sec = 1; sec < vesting_seconds; ++sec) {
      BOOST_CHECK(!vesting_balance.is_withdraw_allowed(beginning + sec, initial_balance));
   }
   BOOST_CHECK(vesting_balance.is_withdraw_allowed(beginning + vesting_seconds, initial_balance));
}

BOOST_FIXTURE_TEST_CASE(when_seconds_passes_allowed_withdraw_increases_proportionally, vesting_balance_fixture)
{
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning).amount.value, 0);
   // initial vesting balance and vesting seconds set to 100 for easy testing
   for (uint32_t i = 1; i < vesting_seconds; ++i) {
      BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + i).amount.value, i);
   }
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + vesting_seconds).amount.value, initial_balance.amount.value);
}

BOOST_FIXTURE_TEST_CASE(when_deposit_the_respective_portion_is_vested_as_time_passes, vesting_balance_fixture)
{
   share_type vested_amount;
   uint32_t delta_seconds = 50;
   share_type allowed_withdraw_after_delta_seconds = delta_seconds; // initial vesting balance and vesting seconds set to 100 for easy testing
   share_type deposit_amount = 100;

   /// we deposit after delta_seconds
   //    precondition
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + delta_seconds).amount.value, allowed_withdraw_after_delta_seconds.value);
   vesting_balance.deposit(beginning + delta_seconds, asset(deposit_amount));
   //    postcondition
   BOOST_CHECK_EQUAL(vesting_balance.balance.amount.value, initial_balance.amount.value + deposit_amount.value);
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + delta_seconds).amount.value, allowed_withdraw_after_delta_seconds.value);

   /// after some more seconds portion of initial balance and portion of deposit amount are vested
   uint32_t check_point = delta_seconds + 10;
   vested_amount = initial_balance.amount * check_point / vesting_seconds;
   vested_amount += deposit_amount * (check_point - delta_seconds) / vesting_seconds;
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + check_point).amount.value, vested_amount.value);

   /// after vesting seconds the initial balance is vested + portion of the new deposit
   vested_amount = initial_balance.amount + (vesting_seconds - delta_seconds) * deposit_amount / vesting_seconds;
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + vesting_seconds).amount.value, vested_amount.value);

   /// after vesting seconds + delta_seconds the total balance is vested
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + vesting_seconds + delta_seconds).amount.value, (initial_balance.amount + deposit_amount).value);
}

BOOST_FIXTURE_TEST_CASE(when_deposit_is_added_allowed_withdraw_increases, vesting_balance_fixture)
{
   uint32_t delta_seconds = 10;
   share_type deposit_amount = 100;
   asset previous_allowed_withdraw = asset(0);
   /// reset initial balance
   vesting_balance.balance = asset(0);
   for (uint32_t step = delta_seconds; step < vesting_seconds;) {
      fc::time_point_sec point = beginning + step;
      vesting_balance.deposit(point, asset(deposit_amount));
      if (step > delta_seconds)
         BOOST_CHECK_GT(vesting_balance.get_allowed_withdraw(point).amount.value, previous_allowed_withdraw.amount.value);
      previous_allowed_withdraw = vesting_balance.get_allowed_withdraw(point);
      step += delta_seconds;
   }
}

BOOST_FIXTURE_TEST_CASE(when_witdraw_total_vested_amount_withdraw_not_allowed, vesting_balance_fixture)
{
   uint32_t delta_seconds = 10;
   share_type withdraw_amount = vesting_balance.get_allowed_withdraw(beginning + delta_seconds).amount;
   vesting_balance.withdraw(beginning + delta_seconds, asset(withdraw_amount));
   BOOST_CHECK_EQUAL(vesting_balance.balance.amount.value, initial_balance.amount.value - withdraw_amount.value);
   BOOST_CHECK_GT(vesting_balance.balance.amount.value, 0);
   BOOST_CHECK_EQUAL(vesting_balance.get_allowed_withdraw(beginning + delta_seconds).amount.value, 0);
}

BOOST_FIXTURE_TEST_CASE(when_vested_portion_withdrawn_no_exception, vesting_balance_fixture)
{
   uint32_t delta_seconds = 10;
   for (uint32_t step = delta_seconds; step < vesting_seconds;) {
      share_type withdraw_amount = vesting_balance.get_allowed_withdraw(beginning + step).amount;
      vesting_balance.withdraw(beginning + step, asset(withdraw_amount));
      step += delta_seconds;
   }
   BOOST_CHECK_GT(vesting_balance.balance.amount.value, 0);
}

BOOST_FIXTURE_TEST_CASE(test, vesting_balance_fixture)
{
   share_type withdraw_amount = 50;
   share_type deposit_amount = 10;
   uint32_t withdraw_delta_seconds = 50;
   uint32_t deposit_delta_seconds = 10;
   uint32_t check_point_delta_seconds = 10;

   /// we withdraw at point +withdraw_delta_seconds
   vesting_balance.withdraw(beginning + withdraw_delta_seconds, asset(withdraw_amount));
   /// we deposit at point +(withdraw_delta_seconds + deposit_delta_seconds)
   vesting_balance.deposit(beginning + withdraw_delta_seconds + deposit_delta_seconds, asset(deposit_amount));
   /// we check at point +(withdraw_delta_seconds + deposit_delta_seconds + check_point_delta_seconds)
   auto allowed_withdraw = vesting_balance.get_allowed_withdraw(beginning + withdraw_delta_seconds + deposit_delta_seconds + check_point_delta_seconds);
   /// we expect initial balance - withdrawal portion to be vested
   share_type expected_allowed_withdraw = (initial_balance.amount - withdraw_amount) * (deposit_delta_seconds + check_point_delta_seconds) / vesting_seconds;
   /// we expect deposit portion to be vested
   expected_allowed_withdraw += deposit_amount * check_point_delta_seconds / vesting_seconds;

   BOOST_CHECK_EQUAL(allowed_withdraw.amount.value, expected_allowed_withdraw.value);
}

BOOST_AUTO_TEST_SUITE_END()


#include "OccupancyGrid/loadData.h"
#include "OccupancyGrid/cvmat_serialization.h"
#include <boost/typeof/typeof.hpp>
#include <cstdio>

using namespace std;
using namespace gtsam;

void loadDataFromTxt(const std::string& odometry_fname,
    const std::string& snapshot_fname,
    vector<Pose2>& allposes,
    vector<double>& ranges,
    double& max_dist)
{
  //======== load odometry ==========//
  vector<Pose2> pose;
  FILE *fptr = fopen(odometry_fname.c_str(), "r");
  while (!feof(fptr)) {
    double x, y, theta;
    if (fscanf(fptr, "%lf %lf %lf", &x, &y, &theta) != 3)
      throw std::runtime_error("error while fscanf");
    theta *= 3.174159 / 180.0;
    pose.push_back(*(new Pose2(x, y, theta)));
  }
  fclose(fptr);

  //======= load laser scans ===============//
  fptr = fopen(snapshot_fname.c_str(), "r");
  if (fptr == NULL) {
    printf("Cannot open file...exiting\n");
    exit(1);
  }

  double min_angle, max_angle, step;
  double min_dist;
  int n_readings;

  if (fscanf(fptr, "%lf %lf %lf %lf %lf %d", &min_angle, &max_angle, &step,
      &max_dist, &min_dist, &n_readings) != 6)
    throw std::runtime_error("error while fscanf");

  double range;
  Index it = 0;
  while (!feof(fptr)) {
    double theta = 3.14159 / 2 + pose[it].theta() + min_angle; //compute angle of first laser
    for (int i = 0; i < n_readings && !feof(fptr); i++) {
      Pose2 p(pose[it].y() / 100, pose[it].x(), theta);
      allposes.push_back(p);
      if (fscanf(fptr, "%lf", &range) != 1) //load range
        throw std::runtime_error("error while fscanf");

      ranges.push_back(range);
      theta += step; //increment angle for next laser
    }
    it++;
  }
  fclose(fptr);
  //======= end load laser scans ============//
}

void cvMatToData(const cv::Mat& laser_pose,
    const cv::Mat& laser_range,
    const cv::Mat& scan_angles,
    const cv::Mat& laser_reflectance,
    vector<Pose2>& allposes,
    vector<double>& allranges,
    vector<uint8_t>& allreflectance)
{
  for (int i = 0; i < laser_range.rows; i ++) {
    const double* pose = laser_pose.ptr<double>(i);
    const double* ranges = laser_range.ptr<double>(i);
    const double* angles = scan_angles.ptr<double>(i);
    const uint8_t* reflectance = laser_reflectance.ptr<uint8_t>(i);
    for (int j = 0; j < laser_range.cols; j++) {
      double theta = pose[2] + angles[j];
      Pose2 p(pose[0], pose[1], theta);
      allposes.push_back(p);
      allranges.push_back(ranges[j]);
      allreflectance.push_back(reflectance[j]);
    }
  }
}

void
downsample(cv::Mat& matout,
    cv::Mat& mat,
    int steprows=1, int stepcols=1,
    int ignorerows=0)
{
    int newrows = (mat.rows - ignorerows) / steprows;
    int newcols = mat.cols / stepcols;
    matout.create(newrows, newcols, CV_64F);
    //printf("%d, %d <=> %d\n", CV_32F, CV_64F, mat.type());
    assert(mat.type() == CV_64F);
    int row = 0;
    for (int i = 0; i < (mat.rows - ignorerows); i += steprows) {
        double* row_ptr = mat.ptr<double>(i);
        int col = 0;
        for (int j = 0; j < mat.cols; j += stepcols) {
            matout.at<double>(row, col++) = row_ptr[j];
        }
        row++;
    }
}

void loadPlayerSim(const std::string& laser_pose_file,
    const std::string& laser_range_file,
    const std::string& scan_angles_file,
    const std::string& laser_reflectance_file,
    std::vector<gtsam::Pose2>& allposes,
    std::vector<double>& allranges,
    std::vector<uint8_t>& allreflectance)
{
  cv::Mat laser_pose, laser_range, scan_angles, laser_reflectance;
  loadMat(laser_pose, laser_pose_file);
  loadMat(laser_range, laser_range_file);
  loadMat(scan_angles, scan_angles_file);
  loadMat(laser_reflectance, laser_reflectance_file);

  // cv::Mat laser_pose_d, laser_range_d, scan_angles_d;
  // // downsample poses, downsample lasers, skip last few observations
  // downsample(laser_pose_d, laser_pose,   2, 1,  0);
  // downsample(laser_range_d, laser_range, 2, 10, 0);
  // downsample(scan_angles_d, scan_angles, 2, 10, 0);

  cvMatToData(
      laser_pose, laser_range, scan_angles, laser_reflectance,
      allposes, allranges, allreflectance);
}

void shiftPoses(
    cv::Mat& laser_pose,
    const cv::Mat& scan_angles,
    const cv::Mat& laser_range,
    const cv::Mat& laser_reflectance,
    double& width,
    double& height,
    double& origin_x,
    double& origin_y)
{
  std::vector<gtsam::Pose2> allposes;
  std::vector<double> allranges;
  std::vector<uint8_t> allreflectance;
  cvMatToData(
      laser_pose, laser_range, scan_angles, laser_reflectance,
      allposes, allranges, allreflectance);
  shiftPoses(allranges, allposes, width, height, origin_x, origin_y);
  for (int r = 0; r < laser_pose.rows; r++) {
    double* pose_robot = laser_pose.ptr<double>(r);
    pose_robot[0] = pose_robot[0] - origin_x;
    pose_robot[1] = pose_robot[1] - origin_y;
  }
}

void shiftPoses(
    const std::vector<double>& allranges,
    std::vector<gtsam::Pose2>& allposes,
    double& width,
    double& height,
    double& origin_x,
    double& origin_y)
{
  BOOST_AUTO(pose, allposes.begin());
  BOOST_AUTO(range, allranges.begin());
  double min_x = std::numeric_limits<double>::infinity(),
         max_x = - std::numeric_limits<double>::infinity(),
         min_y = std::numeric_limits<double>::infinity(),
         max_y = - std::numeric_limits<double>::infinity();
  for (; pose != allposes.end() && range != allranges.end(); ++pose, ++range)
  {
    double end_x = pose->x() + *range * cos(pose->theta()),
           end_y = pose->y() + *range * sin(pose->theta());
    min_x = std::min(std::min(end_x, min_x), pose->x());
    min_y = std::min(std::min(end_y, min_y), pose->y());
    max_x = std::max(std::max(end_x, max_x), pose->x());
    max_y = std::max(std::max(end_y, max_y), pose->y());
  }
  double margin_x = 1, margin_y = 1;
  max_x += margin_x;
  max_y += margin_y;
  min_x -= margin_x;
  min_y -= margin_y;
  double mid_x = (max_x + min_x) / 2;
  double mid_y = (max_y + min_y) / 2;
  pose = allposes.begin();
  for (; pose != allposes.end(); ++pose) {
    *pose = gtsam::Pose2( pose->x() - mid_x, pose->y() - mid_y, pose->theta());
  }
  width = max_x - min_x;
  height = max_y - min_y;
}


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

  @copyright 2016 J.T.Lapreste

  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_FUNCTION_DIV_INCLUDED
#define BOOST_SIMD_FUNCTION_DIV_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-operator

    Perform the quotient of two parameters of the same type with or without options.

    @par Semantic

    For any value @c a and @c b of type @c T,

    @code
    T r = div(a, b{, option});
    @endcode

    returns the quotient of @c a by @c b according to the option

    if there is no option the call is equivalent to divides(a, b),  else
    option can be ceil, floor, fix, round, round2even (in the namespace booost::simd)
    and provide the same result as the calls divceil(a, b), divfloor(a, b),
    divfix(a, b), divround(a, b).

    @param a First  parameter of the quotient
    @param b Second parameter of the quotient
    @param option is the option

    @return The quotient of the two parameters.
  **/
  template<typename T> auto div(T const& a, T const& b);

  namespace functional
  {
    /*!
      @ingroup group-callable-operator

      Perform the quotient of two parameters of the same type.

      Function object tied to boost::simd::div

      @see boost::simd::div
    **/
    const boost::dispatch::functor<tag::div_> div = {};
  }
} }
#endif

#include <boost/simd/function/scalar/div.hpp>

#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <boost/graph/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <algorithm>
#include <boost/pending/fibonacci_heap.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/pending/indirect_cmp.hpp>

using namespace boost;

int
main()
{
  typedef indirect_cmp<float*,std::less<float> > ICmp;
  int i;
  boost::mt19937 gen;
  for (int N = 2; N < 200; ++N) {
    uniform_int<> distrib(0, N-1);
    variate_generator<boost::mt19937&, uniform_int<> > rand_gen(gen, distrib);
    for (int t = 0; t < 10; ++t) {
      std::vector<float> v, w(N);

      ICmp cmp(&w[0], std::less<float>());
      fibonacci_heap<int, ICmp> Q(N, cmp);

      for (int c = 0; c < w.size(); ++c)
        w[c] = c;
      std::random_shuffle(w.begin(), w.end());

      for (i = 0; i < N; ++i)
        Q.push(i);

      for (i = 0; i < N; ++i) {
        int u = rand_gen();
        float r = rand_gen(); r /= 2.0;
        w[u] = w[u] - r;
        Q.update(u);
      }

      for (i = 0; i < N; ++i) {
        v.push_back(w[Q.top()]);
        Q.pop();
      }
      std::sort(w.begin(), w.end());

      if (! std::equal(v.begin(), v.end(), w.begin())) {
        std::cout << std::endl << "heap sorted: ";
        std::copy(v.begin(), v.end(),
                  std::ostream_iterator<float>(std::cout," "));
        std::cout << std::endl << "correct: ";
        std::copy(w.begin(), w.end(),
                  std::ostream_iterator<float>(std::cout," "));
        std::cout << std::endl;
        return -1;
      }
    }
  }
  std::cout << "fibonacci heap passed test" << std::endl;
  return 0;
}


#include "../DynAutoDiff/Distributions.hpp"
#include "../DynAutoDiff/Var.hpp"
#include <algorithm>
#include <boost/test/tools/old/interface.hpp>
#include <eigen3/Eigen/Core>
#include <iostream>
#include <ostream>
#include <vector>

#define BOOST_TEST_MODULE Normal_Test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#define TL 1e-10

using namespace Eigen;
using namespace std;
using namespace DynAutoDiff;

BOOST_AUTO_TEST_SUITE(Initialize_Variable_Test)

BOOST_AUTO_TEST_CASE(init_var_test) {
    auto X = load("X.txt");

    BOOST_CHECK_EQUAL(X->rows(), 1000);
    BOOST_CHECK_EQUAL(X->cols(), 5);
    BOOST_CHECK_CLOSE(X->v(0, 0), -0.343003152130103, TL);
    BOOST_CHECK_CLOSE(X->v(5, 4), -0.0470153684815493, TL);
    BOOST_CHECK_CLOSE(X->v(99, 4), 0.303972116239077, TL);

    auto y = load("y.txt");

    BOOST_CHECK_EQUAL(y->rows(), 1000);
    BOOST_CHECK_EQUAL(y->cols(), 1);
    BOOST_CHECK_CLOSE(y->v(99, 0), -1.70565299307618, TL);
}
BOOST_AUTO_TEST_CASE(ivecl_test) {}
BOOST_AUTO_TEST_CASE(times_test) {}

BOOST_AUTO_TEST_CASE(division_test) {}
BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <fstream>
#include <boost/graph/adjacency_list.hpp>

using namespace boost;

template < typename Graph > void
read_graph_file(std::istream & in, Graph & g)
{
  typedef typename graph_traits < Graph >::vertex_descriptor Vertex;
  typedef typename graph_traits < Graph >::vertices_size_type size_type;
  size_type n_vertices;
  in >> n_vertices;             // read in number of vertices
  std::vector < Vertex > vertex_set(n_vertices);
  for (size_type i = 0; i < n_vertices; ++i)
    vertex_set[i] = add_vertex(g);

  size_type u, v;
  while (in >> u)
    if (in >> v)
      add_edge(vertex_set[u], vertex_set[v], g);
    else
      break;
}

int
main()
{
  typedef adjacency_list < listS,       // Store out-edges of each vertex in a std::list
    vecS,                       // Store vertex set in a std::vector
    directedS                   // The graph is directed
  > graph_type;

  graph_type g;                 // use default constructor to create empty graph
  std::ifstream file_in("makefile-dependencies.dat");
  read_graph_file(file_in, g);

  assert(num_vertices(g) == 15);
  assert(num_edges(g) == 19);
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.exponential toolbox - tanh/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of exponential components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 08/12/2010
///
#include <nt2/include/functions/tanh.hpp>
#include <nt2/include/functions/sin.hpp>
#include <nt2/include/functions/mul_i.hpp>
#include <nt2/include/functions/mul_minus_i.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>

NT2_TEST_CASE_TPL ( tanh_real__1_0, NT2_REAL_TYPES)
{

  using nt2::tanh;
  using nt2::tag::tanh_;
  typedef std::complex<T> cT;
  typedef cT r_t;
  double ulpd;
  ulpd=0.0;
  const int N = 20;
  cT inputs[N] =
    { cT(nt2::Zero<T>(),nt2::Zero<T>()), //0
      cT(nt2::Inf<T>(),nt2::Zero<T>()),  //1
      cT(nt2::Minf<T>(),nt2::Zero<T>()), //2
      cT(nt2::Nan<T>(),nt2::Zero<T>()),  //3
      cT(nt2::Zero<T>(),nt2::Inf<T>()),  //4
      cT(nt2::Inf<T>(),nt2::Inf<T>()),   //5
      cT(nt2::Minf<T>(),nt2::Inf<T>()),  //6
      cT(nt2::Nan<T>(),nt2::Inf<T>()),   //7
      cT(nt2::Zero<T>(),nt2::Minf<T>()), //8
      cT(nt2::Inf<T>(),nt2::Minf<T>()),  //9
      cT(nt2::Minf<T>(),nt2::Minf<T>()), //10
      cT(nt2::Nan<T>(),nt2::Minf<T>()),  //11
      cT(nt2::Zero<T>(),nt2::Nan<T>()),  //12
      cT(nt2::Inf<T>(),nt2::Nan<T>()),   //13
      cT(nt2::Minf<T>(),nt2::Nan<T>()),  //14
      cT(nt2::Nan<T>(),nt2::Nan<T>()),   //15
      cT(nt2::Zero<T>(),nt2::Pi <T>()),  //16
      cT(nt2::Inf<T>(),nt2::Pi <T>()),   //17
      cT(nt2::Minf<T>(),nt2::Pi <T>()),  //18
      cT(nt2::Nan<T>(),nt2::Pi <T>()),   //19
    };

  cT results[N] =
    { cT(nt2::Zero<T>(),nt2::Zero<T>()),//0
      cT(nt2::Nan<T>(),nt2::Zero<T>()), //1
      cT(nt2::Nan<T>(),nt2::Zero<T>()), //2
      cT(nt2::Nan<T>(),nt2::Zero<T>()), //3
      cT(nt2::Zero<T>(),nt2::Nan<T>()), //4
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //5
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //6
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //7
      cT(nt2::Zero<T>(),nt2::Nan<T>()), //8
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //9
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //10
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //11
      cT(nt2::Zero<T>(),nt2::Nan<T>()), //12
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //13
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //14
      cT(nt2::Nan<T>(),nt2::Nan<T>()),  //15
      cT(nt2::Zero<T>(),nt2::Zero<T>()),//16
      cT(nt2::Nan<T>(),nt2::Zero<T>()), //17
      cT(nt2::Nan<T>(),nt2::Zero<T>()), //18
      cT(nt2::Nan<T>(),nt2::Nan <T>()), //19
    };

  for(int i=0; i < N; i++)
   {
     std::cout << i << "  input = " << inputs[i] << " -> " << results[i] << " <-> " << nt2::tanh(inputs[i]) << std::endl;
     NT2_TEST_ULP_EQUAL(nt2::tanh(inputs[i]), results[i], 1);
     NT2_TEST_ULP_EQUAL(nt2::tanh(-inputs[i]), -results[i], 1);
   }

} // end of test for floating_


/* Copyright (c) 2019, Acme Robotics, Ethan Quist, Corbyn Yhap
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <organization> nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * @file StraightLinePath.cpp
 *
 * @brief This is the source code for the straight line path generator.
 * The class can take in start point and end point transformations
 * and return a vector of the points along that path. The path between
 * the start point and end point will be a linear path that is broken
 * up by the increment amount given as an input.
 */

#include <Eigen/Dense>

#include "StraightLinePath.hpp"

StraightLinePath::StraightLinePath() {
}

StraightLinePath::~StraightLinePath() {
}

std::vector<Eigen::Matrix4d> StraightLinePath::computePath(
    const Eigen::Matrix4d &aStartMat, const Eigen::Matrix4d &aEndMat,
    const double &aIncrement) {

  Coordinate tStart(aStartMat(0, 3), aStartMat(1, 3), aStartMat(2, 3));
  Coordinate tEnd(aEndMat(0, 3), aEndMat(1, 3), aEndMat(2, 3));

  // Store end vector value as it will be used frequently.
  Eigen::Vector3d tEndVec = tEnd.getAsVec();

  // Initialize Points with Start Point
  std::vector<Eigen::Matrix4d> points;
  points.push_back(aStartMat);
  // Compute the distance to the goal from our last point
  double tDistance = (tEndVec - tStart.getAsVec()).norm();

  // Note: Because this is a straight line path the unit vector only needs to be
  // computed once.

  // Determine the direction to the goal
  Eigen::Vector3d unitVec = determineDirection(tStart, tEnd, aIncrement);

  // This algorithm assumes that the orientation stays constant from
  // start to end. So we copy the start matrix and replace the positions.
  while (tDistance > aIncrement) {
    Coordinate tNewLoc(0, 0, 0);
    tNewLoc.setX(unitVec(0) * aIncrement + points.back()(0, 3));
    tNewLoc.setY(unitVec(1) * aIncrement + points.back()(1, 3));
    tNewLoc.setZ(unitVec(2) * aIncrement + points.back()(2, 3));
    Eigen::Matrix4d tNewPoint = aStartMat;

    tNewPoint.block<3, 1>(0, 3) = tNewLoc.getAsVec();
    // Put the newly calculated point on the list

    points.push_back(tNewPoint);

    // Recompute the new distance
    tDistance = (tEndVec - points.back().block<3, 1>(0, 3)).norm();
  }
  // Finally add the end point
  points.push_back(aEndMat);

  return points;
}

Eigen::Vector3d StraightLinePath::determineDirection(const Coordinate &aStart,
                                                     const Coordinate &aEnd,
                                                     const double &aIncrement) {
  Eigen::Vector3d tPnt;
  // Compute the pointing vector
  tPnt = aEnd.getAsVec() - aStart.getAsVec();

  // Normalize the pointing vector for a unit vector
  tPnt.normalize();

  // Increment is not needed for this path planner to determine the direction.
  // Assuming no obstacles to avoid.
  (void) aIncrement;

  return tPnt;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021, LAAS-CNRS, New York University, Max Planck Gesellschaft,
//                     University of Edinburgh, University of Trento
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
#define CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/control-base.hpp"

namespace crocoddyl {

template <typename _Scalar>
class ControlParametrizationModelNumDiffTpl : public ControlParametrizationModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ControlParametrizationModelAbstractTpl<_Scalar> Base;
  typedef ControlParametrizationDataNumDiffTpl<_Scalar> Data;
  typedef ControlParametrizationDataAbstractTpl<_Scalar> ControlParametrizationDataAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Construct a new ControlParametrizationModelNumDiff object
   *
   * @param model
   */
  explicit ControlParametrizationModelNumDiffTpl(boost::shared_ptr<Base> model);
  virtual ~ControlParametrizationModelNumDiffTpl();

  /**
   * @brief Get the value of the control at the specified time
   *
   * @param[in]  data   Control-parametrization numdiff data
   * @param[in]  t      Time in [0,1]
   * @param[in]  u      Control parameters
   */
  void calc(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
            const Eigen::Ref<const VectorXs>& u) const;

  /**
   * @brief Get the value of the Jacobian of the control with respect to the parameters
   *
   * @param[in]  data   Control-parametrization numdiff data
   * @param[in]  t      Time in [0,1]
   * @param[in]  u      Control parameters
   */
  void calcDiff(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
                const Eigen::Ref<const VectorXs>& u) const;

  /**
   * @brief Create the control-parametrization data
   *
   * @return the control-parametrization data
   */
  virtual boost::shared_ptr<ControlParametrizationDataAbstract> createData();

  /**
   * @brief Get a value of the control parameters such that the control at the specified time
   * t is equal to the specified value u
   *
   * @param[in]  data   Control-parametrization numdiff data
   * @param[in]  t      Time in [0,1]
   * @param[in]  w      Control values
   */
  void params(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
              const Eigen::Ref<const VectorXs>& w) const;

  /**
   * @brief Convert the bounds on the control to bounds on the control parameters
   *
   * @param[in]  w_lb   Control lower bound
   * @param[in]  w_ub   Control upper bound
   * @param[in]  u_lb   Control parameter lower bound
   * @param[in]  u_ub   Control parameter upper bound
   */
  void convertBounds(const Eigen::Ref<const VectorXs>& w_lb, const Eigen::Ref<const VectorXs>& w_ub,
                     Eigen::Ref<VectorXs> u_lb, Eigen::Ref<VectorXs> u_ub) const;

  /**
   * @brief Compute the product between a specified matrix and the Jacobian of the control (with respect to the
   * parameters)
   *
   * @param[in]  data   Control-parametrization numdiff data
   * @param[in]  A      A matrix to multiply times the Jacobian
   * @param[out] out    Product between the matrix A and the Jacobian of the control with respect to the parameters
   * @param[in] op      Assignment operator which sets, adds, or removes the given results
   */
  void multiplyByJacobian(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
                          const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
                          const AssignmentOp = setto) const;

  /**
   * @brief Compute the product between the transposed Jacobian of the control (with respect to the parameters) and
   * a specified matrix
   *
   * @param[in]  data   Control-parametrization numdiff data
   * @param[in]  A      A matrix to multiply times the Jacobian
   * @param[out] out    Product between the transposed Jacobian of the control with respect to the parameters and the
   * matrix A
   * @param[in] op      Assignment operator which sets, adds, or removes the given results
   */
  void multiplyJacobianTransposeBy(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
                                   const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
                                   const AssignmentOp = setto) const;

  /**
   * @brief Get the model_ object
   *
   * @return Base&
   */
  const boost::shared_ptr<Base>& get_model() const;

  /**
   * @brief Get the disturbance_ object
   *
   * @return Scalar
   */
  const Scalar get_disturbance() const;

  /**
   * @brief Set the disturbance_ object
   *
   * @param disturbance is the value used to find the numerical derivative
   */
  void set_disturbance(const Scalar disturbance);

 private:
  /**
   * @brief This is the control we need to compute the numerical differentiation
   * from.
   */
  boost::shared_ptr<Base> model_;

  /**
   * @brief This the increment used in the finite differentiation and integration.
   */
  Scalar disturbance_;

 protected:
  using Base::nu_;
  using Base::nw_;
};

template <typename _Scalar>
struct ControlParametrizationDataNumDiffTpl : public ControlParametrizationDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
  typedef ControlParametrizationDataAbstractTpl<Scalar> Base;

  template <template <typename Scalar> class Model>
  explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>* const model) : Base(model), du(model->get_nu()) {
    du.setZero();

    const std::size_t nu = model->get_model()->get_nu();
    data_0 = model->get_model()->createData();
    for (std::size_t i = 0; i < nu; ++i) {
      data_u.push_back(model->get_model()->createData());
    }
  }

  virtual ~ControlParametrizationDataNumDiffTpl() {}

  VectorXs du;                                   //!< temporary variable used for finite differencing
  boost::shared_ptr<Base> data_0;                //!< The data that contains the final results
  std::vector<boost::shared_ptr<Base> > data_u;  //!< The temporary data associated with the control variation
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/numdiff/control.hxx"

#endif  // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_


//
// estimators.hpp
//
//  Created by Peter Gusev on 19 April 2016.
//  Copyright 2013-2016 Regents of the University of California
//

#ifndef __estimators_h__
#define __estimators_h__

#include <stdlib.h>
#include <assert.h>
#include <deque>
#include <boost/shared_ptr.hpp>
#include <boost/move/move.hpp>

#include "ndnrtc-defines.hpp"

namespace ndnrtc {
	namespace estimators {
		/**
		 * Interface for estimator window class.
		 * An estimator window defines an interval in some dimension, over
		 * which estimator is operating. For instance, if
		 * estimator window is in time dimension and has value of 5 seconds,
		 * then estimator is operating over window of 5 seconds. Likewise,
		 * if window's dimension is the number of samples, then estimator is
		 * operating over the window of 5 samples.
		 */
		class IEstimatorWindow {
		public:
			/**
			 * Indicates progress over the window.
			 * Must be called every time estimator receives a new value.
			 * @return true if window limit has been reached, false otherwise
			 */
			virtual bool isLimitReached() = 0;

            /**
             * Cuts provided sample array to be of window size
             */
            virtual void cut(std::deque<double>& samples) = 0;
		};

		class SampleWindow : public IEstimatorWindow {
		public:
			SampleWindow(unsigned int nSamples):nSamples_(nSamples),remaining_(nSamples)
			{ assert(nSamples_); }

			bool isLimitReached();
            void cut(std::deque<double>& samples);
		private:
			unsigned int nSamples_, remaining_;
		};

		class TimeWindow : public IEstimatorWindow {
		public:
			TimeWindow(unsigned int milliseconds);

			bool isLimitReached();
            void cut(std::deque<double>& samples);
		private:
			unsigned int milliseconds_;
			int64_t lastReach_;
		};

		/**
		 * Base class for estimators
		 */
		class Estimator {
		public:
			Estimator(std::shared_ptr<IEstimatorWindow> window):
				value_(0),window_(window),nValues_(0){}

			virtual void newValue(double value) = 0;
			virtual double value() const { return value_; }
			unsigned int count() const { return nValues_; }

		protected:
			unsigned int nValues_;
			double value_;
			std::shared_ptr<IEstimatorWindow> window_;
		};

		/**
		 * Sliding window estimator calculates average and deviation over time
		 * window
		 */
		class Average : public Estimator {
		public:
			Average(std::shared_ptr<IEstimatorWindow> window);

			void newValue(double value);
			double deviation() const { return sqrt(variance_); }
			double variance() const { return variance_; }
            double oldestValue() const { return (samples_.size() ? samples_.front() : 0); }
            double latestValue() const { return (samples_.size() ? samples_.back() : 0); }

		private:
			bool limitReached_;
			std::deque<double> samples_;
			double accumulatedSum_, variance_;
		};

		/**
		 * Frequency estimator measures average frequency (per second) of new value
		 * appearings. Meter value is updated every window interval.
		 */
		class FreqMeter : public Estimator {
		public:
			FreqMeter(std::shared_ptr<IEstimatorWindow> window);

			/**
			 * Passed value is ignored. This call is used to calculate frequency of
			 * calling this method over the estimator window.
			 */
			void newValue(double value);

		private:
            std::deque<double> samples_;
            bool run_;
		};

		/**
		 * A low pass filter class
		 */
		class Filter {
		public:
			Filter(double smoothing = 1./8.);

			double value() const { return value_; }
			void newValue(double value);

		private:
			double smoothing_,value_;
		};
	}
}

#endif


#include <boost/random.hpp>


#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/BreitWigner.h"
#include "PhysicsTools/Utilities/interface/Constant.h"
#include "PhysicsTools/Utilities/interface/Number.h"
#include "PhysicsTools/Utilities/interface/Product.h"
#include "PhysicsTools/Utilities/interface/Sum.h"
#include "PhysicsTools/Utilities/interface/Difference.h"
#include "PhysicsTools/Utilities/interface/Gaussian.h"
#include "PhysicsTools/Utilities/interface/ZLineShape.h"
#include "PhysicsTools/Utilities/interface/Convolution.h"
#include "PhysicsTools/Utilities/interface/HistoChiSquare.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/RootFunctionAdapter.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "TFile.h"
#include "TH1.h"
#include "TF1.h"
#include "TMath.h"
#include "TCanvas.h"
#include "TROOT.h"
//#include "TStyle.h"
#include <boost/shared_ptr.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <algorithm>
#include <exception>
#include <iterator>
#include <string>
#include <vector>
using namespace std;
using namespace boost;
namespace po = boost::program_options;

// A helper function to simplify the main part.
template<class T>
ostream& operator<<(ostream& os, const vector<T>& v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

typedef funct::GaussIntegrator IntegratorConv;

int main(int ac, char *av[]) {
  try {
    double fMin, fMax;
    string ext;
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help", "produce help message")
      ("include-path,I", po::value< vector<string> >(),
       "include path")
      ("input-file", po::value< vector<string> >(), "input file")
      ("min,m", po::value<double>(&fMin)->default_value(80), "minimum value for fit range")
      ("max,M", po::value<double>(&fMax)->default_value(120), "maximum value for fit range")
      ("breitwigner", "fit to a breit-wigner")
      ("gauss", "fit to a gaussian")
      ("bwinter", "fit to the breit-wigner plus interference term")
      ("bwintgam", "fit to the breit-wigner plus interference term and gamma propagator")
      ("convbwg", "fit to the convolution between a breit-wigner and a gaussian")
      ("convbwinterg",
       "fit to the convolution between a breit-wigner plus interference term and a gaussian")
      ("convbwintgamg",
       "fit to the convolution of a breit-wigner plus interference term and gamma propagator and a gaussian")
      ("convbw2gf",
       "fit to the convolution between a breit-wigner and a linear combination of fixed gaussians")
      ("convbwf2g",
       "fit to the convolution between a fixed breit-wigner and a linear combination of gaussians")
      ("convbwint2gf",
       "fit to the convolution between the breit-wigner plus interference term and a linear combination of fixed gaussians")
      ("convbwintf2g",
       "fit to the convolution between the fixed breit-wigner plus interference term and a linear combination of gaussians")
      ("convbwintgam2gf",
       "fit to the convolution of the breit-wigner plus interference term and gamma propagator with a linear combination of fixed gaussians")
      ("convbwintgamf2g",
       "fit to the convolution of the fixed breit-wigner plus interference term and gamma propagator with a linear combination of gaussians")
      ("output-file,O", po::value<string>(&ext)->default_value(".ps"),
       "output file format")
      ;

    po::positional_options_description p;
    p.add("input-file", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
	      options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
    }

    if (vm.count("include-path")) {
      cout << "Include paths are: "
	   << vm["include-path"].as< vector<string> >() << "\n";
    }

    vector<string> v_file;
    vector<TH1D*> v_ZMassHistos;
    vector<string> v_eps;

    if (vm.count("input-file")) {
      cout << "Input files are: "
	   << vm["input-file"].as< vector<string> >() << "\n";
      v_file = vm["input-file"].as< vector<string> >();
      for(vector<string>::const_iterator it = v_file.begin();
	  it != v_file.end(); ++it) {
	TFile * root_file = new TFile(it->c_str(),"read");
	TDirectory *Histos = (TDirectory*) root_file->GetDirectory("ZHisto");
	TDirectory *RecoHistos = (TDirectory*) Histos->GetDirectory("ZRecoHisto");
	TH1D * zMass = (TH1D*) RecoHistos->Get("ZMass");
	zMass->Rebin(4); //remember...
	zMass->GetXaxis()->SetTitle("#mu #mu invariant mass (GeV/c^{2})");
	v_ZMassHistos.push_back(zMass);
	gROOT->SetStyle("Plain");
	//gStyle->SetOptFit(1111);
	string f_string = *it;
	replace(f_string.begin(), f_string.end(), '.', '_');
	string eps_string = f_string + ext;
	v_eps.push_back(eps_string);
	cout << ">>> histogram loaded\n";
      }
      cout << v_file.size() << ", " << v_ZMassHistos.size() << ", " << v_eps.size() << endl;
      cout <<">>> Input files loaded\n";
    }

    IntegratorConv integratorConv(1.e-5);

    //PDG values for Z mass and width
    funct::Parameter mass("Mass", 91.1876);
    funct::Parameter gamma("Gamma", 2.4952);
    //Parameters for Z Line Shape
    funct::Parameter f_gamma("Photon factor", 0);
    funct::Parameter f_int("Interference factor", 0.001);
    //Parameters for fits with gaussians
    funct::Parameter yield("Yield", 482000);
    funct::Parameter alpha("Alpha", 0.771);
    funct::Parameter mean("Mean", 0); //0.229
    funct::Parameter sigma1("Sigma 1", 1.027);
    funct::Parameter sigma2("Sigma 2", 2.94);
    funct::Constant c_yield(yield), c_alpha(alpha);
    funct::Number _1(1);

    if (vm.count("breitwigner")) {
      cout << "Fitting histograms in input file to a Breit-Wigner\n";
      cout << ">>> set pars: " << endl;
      cout << yield << endl;
      cout << mass << endl;
      cout << gamma << endl;
      for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	cout << ">>> load histogram\n";
	TH1D * zMass = v_ZMassHistos[i];
	cout << ">>> histogram loaded\n";
	funct::BreitWigner bw(mass, gamma);
	funct::Constant c_yield(yield);
	typedef funct::Product<funct::Constant, funct::BreitWigner>::type FitFunction;
	FitFunction f = c_yield * bw;
	typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	ChiSquared chi2(f, zMass, fMin, fMax);
	int fullBins = chi2.numberOfBins();
	cout << "N. deg. of freedom: " << fullBins << endl;
	fit::RootMinuit<ChiSquared> minuit(chi2, true);
	minuit.addParameter(yield, 10, 100, 100000);
	minuit.addParameter(mass, .1, 70., 110);
	minuit.addParameter(gamma, 1, 1, 10);
	minuit.minimize();
	minuit.printFitResults();
	TF1 fun = root::tf1("fun", f, fMin, fMax, yield, mass, gamma);
	fun.SetParNames(yield.name().c_str(), mass.name().c_str(), gamma.name().c_str());
	fun.SetLineColor(kRed);
	TCanvas *canvas = new TCanvas("canvas");
	zMass->Draw("e");
	fun.Draw("same");
	string epsFilename = "ZMassFitBW_" + v_eps[i];
	canvas->SaveAs(epsFilename.c_str());
	canvas->SetLogy();
	string epsLogFilename = "ZMassFitBW_Log_" + v_eps[i];
	canvas->SaveAs(epsLogFilename.c_str());
      }
    }

    if (vm.count("gauss")) {
      cout << "Fitting histograms in input files to a Gaussian\n";
      cout << ">>> set pars: " << endl;
      cout << yield << endl;
      cout << mean << endl;
      cout << sigma1 << endl;
      for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	TH1D * zMass = v_ZMassHistos[i];
	funct::Gaussian gaus(mean, sigma1);
	funct::Constant c(yield);
	typedef funct::Product<funct::Constant, funct::Gaussian>::type FitFunction;
	FitFunction f = c * gaus;
	typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	ChiSquared chi2(f, zMass, fMin, fMax);
	int fullBins = chi2.numberOfBins();
	cout << "N. deg. of freedom: " << fullBins << endl;
	fit::RootMinuit<ChiSquared> minuit(chi2, true);
	minuit.addParameter(yield, 10, 100, 100000);
	minuit.addParameter(mean, 0.001, 80, 100);
	minuit.addParameter(sigma1, 0.1, -5., 5.);
	minuit.minimize();
	minuit.printFitResults();
	TF1 fun = root::tf1("fun", f, fMin, fMax, yield, mean, sigma1);
	fun.SetParNames(yield.name().c_str(), mean.name().c_str(), sigma1.name().c_str());
	fun.SetLineColor(kRed);
	TCanvas *canvas = new TCanvas("canvas");
	zMass->Draw("e");
	fun.Draw("same");
	string epsFilename = "ZMassFitG_" + v_eps[i];
	canvas->SaveAs(epsFilename.c_str());
	canvas->SetLogy();
	string epsLogFilename = "ZMassFitG_Log_" + v_eps[i];
	canvas->SaveAs(epsLogFilename.c_str());
      }
    }

    if (vm.count("bwinter")) {
	  cout << "Fitting histograms in input files to the Breit-Wigner plus Z/photon interference term\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << mass << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Constant c(yield);
	    typedef funct::Product<funct::Constant, funct::ZLineShape>::type FitFunction;
	    FitFunction f = c * zls;
	    cout << "set functions" << endl;
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    minuit.fixParameter(f_gamma.name());
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    minuit.minimize();
	    minuit.printFitResults();
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), mass.name().c_str(), gamma.name().c_str(),
	                    f_gamma.name().c_str(), f_int.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitBwIn_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitBwIn_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("bwint"))
	{
	  cout << "Fitting histograms in input files to the Breit-Wigner plus Z/photon interference term and gamma propagator\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << mass << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Constant c(yield);
	    typedef funct::Product<funct::Constant, funct::ZLineShape>::type FitFunction;
	    FitFunction f = c * zls;
	    cout << "set functions" << endl;
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    minuit.minimize();
	    minuit.printFitResults();
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), mass.name().c_str(), gamma.name().c_str(),
	                    f_gamma.name().c_str(), f_int.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitBwInGam_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitBwInGam_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("bwintgam"))
	{
	  cout << "Fitting histograms in input files to the Breit-Wigner plus Z/photon interference term and gamma propagator\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << mass << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Constant c(yield);
	    typedef funct::Product<funct::Constant, funct::ZLineShape>::type FitFunction;
	    FitFunction f = c * zls;
	    cout << "set functions" << endl;
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    minuit.minimize();
	    minuit.printFitResults();
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), mass.name().c_str(), gamma.name().c_str(),
	                    f_gamma.name().c_str(), f_int.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitBwInGam_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitBwInGam_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("convbwg")) {
	  cout << "Fitting histograms in input files to the convolution between a Breit Wigner and a Gaussian\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << mass << endl;
	  cout << gamma << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::BreitWigner bw(mass, gamma);
	    funct::Gaussian gauss(mean, sigma1);
	    double range = 3 * sigma1.value();
	    funct::Convolution<funct::BreitWigner, funct::Gaussian, IntegratorConv>::type cbg(bw, gauss, -range , range, integratorConv);
	    funct::Constant c(yield);
	    typedef funct::Product<funct::Constant, funct::Convolution<funct::BreitWigner, funct::Gaussian, IntegratorConv>::type >::type FitFunction;
	    FitFunction f = c * cbg;
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    minuit.fixParameter(mean.name());
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), mass.name().c_str(), gamma.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwG_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwG_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("convbwinterg"))
	{
	  cout << "Fitting histograms in input files to the convolution between the Breit-Wigner plus Z/photon interference and a Gaussian\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << mass << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Gaussian gauss(mean, sigma1);
	    double range = 3 * sigma1.value();
	    funct::Convolution<funct::ZLineShape, funct::Gaussian, IntegratorConv>::type czg(zls, gauss, -range , range, integratorConv);
	    funct::Constant c(yield);
	    typedef funct::Product<funct::Constant, funct::Convolution<funct::ZLineShape, funct::Gaussian, IntegratorConv>::type >::type FitFunction;
	    FitFunction f = c * czg;
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    minuit.fixParameter(f_gamma.name());
	    minuit.addParameter( f_int, .0001, -1000000, 1000000);
	    minuit.addParameter( mean, 0.001, -0.5, 0.5);
	    minuit.fixParameter(mean.name());
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), mass.name().c_str(), gamma.name().c_str(),
			    f_gamma.name().c_str(), f_int.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwInG_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwInG_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}
      if (vm.count("convbwintgamg"))
	{
	  cout << "Fitting histograms in input files to the convolution of the Breit-Wigner plus Z/photon interference and photon propagator with a Gaussian\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << mass << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Gaussian gauss(mean, sigma1);
	    double range = 3 * sigma1.value();
	    funct::Convolution<funct::ZLineShape, funct::Gaussian, IntegratorConv>::type czg(zls, gauss, -range , range, integratorConv);
	    funct::Constant c(yield);
	    typedef funct::Product<funct::Constant, funct::Convolution<funct::ZLineShape, funct::Gaussian, IntegratorConv>::type >::type FitFunction;
	    FitFunction f = c * czg;
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 10000000);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    minuit.fixParameter(mean.name());
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), mass.name().c_str(), gamma.name().c_str(),
			    f_gamma.name().c_str(), f_int.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str());
	    fun.SetLineColor(kRed);
	    fun.SetNpx(100000);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwInGaG_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwInGaG_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("convbw2gf"))
	{
	  cout << "Fitting histograms in input files to the convolution between the Z Breit-Wigner and a linear combination of fixed Gaussians\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << alpha << endl;
	  cout << mass << endl;
	  cout << gamma << endl;
	  cout << mean << endl;
	  cout << sigma1 << endl;
	  cout << sigma2 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::BreitWigner bw(mass, gamma);
	    funct::Gaussian gaus1(mean, sigma1);
	    funct::Gaussian gaus2(mean, sigma2);
	    typedef funct::Product<funct::Constant, funct::Gaussian>::type G1;
	    typedef funct::Product<funct::Difference<funct::Number,funct::Constant>::type, funct::Gaussian>::type G2;
	    typedef funct::Product<funct::Constant,funct::Sum<G1, G2>::type >::type GaussComb;
	    GaussComb gc = c_yield*(c_alpha*gaus1 + (_1 - c_alpha)*gaus2);
	    typedef funct::Convolution<funct::BreitWigner, GaussComb, IntegratorConv>::type FitFunction;
	    double range = 3 * max(sigma1.value(), sigma2.value());
	    FitFunction f(bw, gc, -range , range, 1000);
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    //minuit.fixParameter(0);
	    minuit.addParameter(alpha, 0.1, -1., 1.);
	    //minuit.fixParameter(1);
	    minuit.addParameter(mass, .1, 70., 110);
	    //minuit.fixParameter(2);
	    minuit.addParameter(gamma, 1, 1, 10);
	    //minuit.fixParameter(3);
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    minuit.fixParameter(mean.name());
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    //minuit.fixParameter(5);
	    minuit.addParameter(sigma2, 0.1, -5., 5.);
	    //minuit.fixParameter(6);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(alpha.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    pars.push_back(sigma2.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), alpha.name().c_str(),
			    mass.name().c_str(), gamma.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str(), sigma2.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwGGf_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwGGf_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("convbwf2g"))
	{
	  cout << "Fitting histograms in input files to the convolution between the fixed Z Breit-Wigner and a linear combination of Gaussians\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << alpha << endl;
	  cout << mass  << endl;
	  cout << gamma << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  cout << sigma2 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::BreitWigner bw(mass, gamma);
	    funct::Gaussian gaus1(mean, sigma1);
	    funct::Gaussian gaus2(mean, sigma2);
	    typedef funct::Product<funct::Constant, funct::Gaussian>::type G1;
	    typedef funct::Product<funct::Difference<funct::Number,funct::Constant>::type, funct::Gaussian>::type G2;
	    typedef funct::Product<funct::Constant,funct::Sum<G1, G2>::type >::type GaussComb;
	    GaussComb gc = c_yield*(c_alpha*gaus1 + (_1 - c_alpha)*gaus2);
	    typedef funct::Convolution<funct::BreitWigner, GaussComb, IntegratorConv>::type FitFunction;
	    double range = 3 * max(sigma1.value(), sigma2.value());
	    FitFunction f(bw, gc, -range , range, 1000);
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    //minuit.fixParameter(0);
	    minuit.addParameter(alpha, 0.1, -1., 1.);
	    //minuit.fixParameter(1);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.fixParameter(mass.name());
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.fixParameter(gamma.name());
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    //minuit.fixParameter(4);
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    //minuit.fixParameter(5);
	    minuit.addParameter(sigma2, 0.1, -10., 10.);
	    //minuit.fixParameter(6);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(alpha.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    pars.push_back(sigma2.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), alpha.name().c_str(),
			    mass.name().c_str(), gamma.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str(), sigma2.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwfGG_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwfGG_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("convbwint2gf"))
	{
	  cout << "Fitting histograms in input files to the convolution between the Breit-Wigner plus Z/photon interference and a linear combination of fixed Gaussians\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << alpha << endl;
	  cout << mass  << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int  << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  cout << sigma2 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Gaussian gaus1(mean, sigma1);
	    funct::Gaussian gaus2(mean, sigma2);
	    typedef funct::Product<funct::Constant, funct::Gaussian>::type G1;
	    typedef funct::Product<funct::Difference<funct::Number,funct::Constant>::type, funct::Gaussian>::type G2;
	    typedef funct::Product<funct::Constant,funct::Sum<G1, G2>::type >::type GaussComb;
	    GaussComb gc = c_yield*(c_alpha*gaus1 + (_1 - c_alpha)*gaus2);
	    typedef funct::Convolution<funct::ZLineShape, GaussComb, IntegratorConv>::type FitFunction;
	    double range = 3 * max(sigma1.value(), sigma2.value());
	    FitFunction f(zls, gc, -range , range, integratorConv);
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    //minuit.fixParameter(0);
	    minuit.addParameter(alpha, 0.1, -1., 1.);
	    //minuit.fixParameter(1);
	    minuit.addParameter(mass, .1, 70., 110);
	    //minuit.fixParameter(2);
	    minuit.addParameter(gamma, 1, 1, 10);
	    //minuit.fixParameter(3);
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    minuit.fixParameter(f_gamma.name());
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    //minuit.fixParameter(5);
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    minuit.fixParameter(mean.name());
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    //minuit.fixParameter(7);
	    minuit.addParameter(sigma2, 0.1, -5., 5.);
	    //minuit.fixParameter(8);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(alpha.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    pars.push_back(sigma2.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), alpha.name().c_str(),
			    mass.name().c_str(), gamma.name().c_str(),
			    f_gamma.name().c_str(), f_int.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str(), sigma2.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwInGGf_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwInGGf_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if(vm.count("convbwintf2g"))
	{
	  cout << "Fitting histograms in input files to the convolution between the fixed Breit-Wigner plus Z/photon interference and a linear combination of Gaussians\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << alpha << endl;
	  cout << mass  << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  cout << sigma2 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Gaussian gaus1(mean, sigma1);
	    funct::Gaussian gaus2(mean, sigma2);
	    typedef funct::Product<funct::Constant, funct::Gaussian>::type G1;
	    typedef funct::Product<funct::Difference<funct::Number,funct::Constant>::type, funct::Gaussian>::type G2;
	    typedef funct::Product<funct::Constant,funct::Sum<G1, G2>::type >::type GaussComb;
	    GaussComb gc = c_yield*(c_alpha*gaus1 + (_1 - c_alpha)*gaus2);
	    typedef funct::Convolution<funct::ZLineShape, GaussComb, IntegratorConv>::type FitFunction;
	    double range = 3 * max(sigma1.value(), sigma2.value());
	    FitFunction f(zls, gc, -range , range, integratorConv);
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    //minuit.fixParameter(0);
	    minuit.addParameter(alpha, 0.1, -1., 1.);
	    //minuit.fixParameter(1);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.fixParameter(mass.name());
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.fixParameter(gamma.name());
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    minuit.fixParameter(f_gamma.name());
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    //minuit.fixParameter(5);
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    //minuit.fixParameter(6);
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    //minuit.fixParameter(7);
	    minuit.addParameter(sigma2, 0.1, -10., 10.);
	    //minuit.fixParameter(8);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(alpha.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    pars.push_back(sigma2.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), alpha.name().c_str(),
			    mass.name().c_str(), gamma.name().c_str(),
			    f_gamma.name().c_str(), f_int.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str(), sigma2.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwInfGG_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwInfGG_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if (vm.count("convbwintgam2gf"))
	{
	  cout << "Fitting histograms in input files to the convolution of the Breit-Wigner plus Z/photon interference and photon propagator with a linear combination of fixed Gaussians\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << alpha << endl;
	  cout << mass  << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int  << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  cout << sigma2 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Gaussian gaus1(mean, sigma1);
	    funct::Gaussian gaus2(mean, sigma2);
	    typedef funct::Product<funct::Constant, funct::Gaussian>::type G1;
	    typedef funct::Product<funct::Difference<funct::Number,funct::Constant>::type, funct::Gaussian>::type G2;
	    typedef funct::Product<funct::Constant,funct::Sum<G1, G2>::type >::type GaussComb;
	    GaussComb gc = c_yield*(c_alpha*gaus1 + (_1 - c_alpha)*gaus2);
	    typedef funct::Convolution<funct::ZLineShape, GaussComb, IntegratorConv>::type FitFunction;
	    double range = 3 * max(sigma1.value(), sigma2.value());
	    FitFunction f(zls, gc, -range , range, integratorConv);
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 10000000);
	    //minuit.fixParameter(0);
	    minuit.addParameter(alpha, 0.1, -1., 1.);
	    //minuit.fixParameter(1);
	    minuit.addParameter(mass, .1, 70., 110);
	    //minuit.fixParameter(2);
	    minuit.addParameter(gamma, 1, 1, 10);
	    //minuit.fixParameter(3);
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    //minuit.fixParameter(4);
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    //minuit.fixParameter(5);
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    minuit.fixParameter(mean.name());
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    //	    minuit.fixParameter(7);
	    minuit.addParameter(sigma2, 0.1, -5., 5.);
	    //	    minuit.fixParameter(8);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(alpha.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    pars.push_back(sigma2.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), alpha.name().c_str(),
			    mass.name().c_str(), gamma.name().c_str(),
			    f_gamma.name().c_str(), f_int.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str(), sigma2.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwInGaGGf_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwInGaGGf_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      if(vm.count("convbwintgamf2g"))
	{
	  cout << "Fitting histograms in input files to the convolution of the fixed Breit-Wigner plus Z/photon interference and photon propagator with a linear combination of Gaussians\n";
	  cout << ">>> set pars: " << endl;
	  cout << yield << endl;
	  cout << alpha << endl;
	  cout << mass  << endl;
	  cout << gamma << endl;
	  cout << f_gamma << endl;
	  cout << f_int << endl;
	  cout << mean  << endl;
	  cout << sigma1 << endl;
	  cout << sigma2 << endl;
	  for(unsigned int i = 0; i < v_ZMassHistos.size(); ++i) {
	    TH1D * zMass = v_ZMassHistos[i];
	    funct::ZLineShape zls(mass, gamma, f_gamma, f_int);
	    funct::Gaussian gaus1(mean, sigma1);
	    funct::Gaussian gaus2(mean, sigma2);
	    typedef funct::Product<funct::Constant, funct::Gaussian>::type G1;
	    typedef funct::Product<funct::Difference<funct::Number,funct::Constant>::type, funct::Gaussian>::type G2;
	    typedef funct::Product<funct::Constant, funct::Sum<G1, G2>::type >::type GaussComb;
	    GaussComb gc = c_yield*(c_alpha*gaus1 + (_1 - c_alpha)*gaus2);
	    typedef funct::Convolution<funct::ZLineShape, GaussComb, IntegratorConv>::type FitFunction;
	    double range = 3 * max(sigma1.value(), sigma2.value());
	    FitFunction f(zls, gc, -range , range, integratorConv);
	    cout << "set functions" << endl;
	    typedef fit::HistoChiSquare<FitFunction> ChiSquared;
	    ChiSquared chi2(f, zMass, fMin, fMax);
	    int fullBins = chi2.numberOfBins();
	    cout << "N. deg. of freedom: " << fullBins << endl;
	    fit::RootMinuit<ChiSquared> minuit(chi2, true);
	    minuit.addParameter(yield, 10, 100, 100000);
	    //minuit.fixParameter(0);
	    minuit.addParameter(alpha, 0.1, -1., 1.);
	    //minuit.fixParameter(1);
	    minuit.addParameter(mass, .1, 70., 110);
	    minuit.fixParameter(mass.name());
	    minuit.addParameter(gamma, 1, 1, 10);
	    minuit.fixParameter(gamma.name());
	    minuit.addParameter(f_gamma, 0.1, -100, 1000);
	    //minuit.fixParameter(4);
	    minuit.addParameter(f_int, .0001, -1000000, 1000000);
	    //minuit.fixParameter(5);
	    minuit.addParameter(mean, 0.001, -0.5, 0.5);
	    //minuit.fixParameter(6);
	    minuit.addParameter(sigma1, 0.1, -5., 5.);
	    //minuit.fixParameter(7);
	    minuit.addParameter(sigma2, 0.1, -10., 10.);
	    //minuit.fixParameter(8);
	    minuit.minimize();
	    minuit.printFitResults();
	    vector<shared_ptr<double> > pars;
	    pars.push_back(yield.ptr());
	    pars.push_back(alpha.ptr());
	    pars.push_back(mass.ptr());
	    pars.push_back(gamma.ptr());
	    pars.push_back(f_gamma.ptr());
	    pars.push_back(f_int.ptr());
	    pars.push_back(mean.ptr());
	    pars.push_back(sigma1.ptr());
	    pars.push_back(sigma2.ptr());
	    TF1 fun = root::tf1("fun", f, fMin, fMax, pars);
	    fun.SetParNames(yield.name().c_str(), alpha.name().c_str(),
			    mass.name().c_str(), gamma.name().c_str(),
			    f_gamma.name().c_str(), f_int.name().c_str(),
			    mean.name().c_str(), sigma1.name().c_str(), sigma2.name().c_str());
	    fun.SetLineColor(kRed);
	    TCanvas *canvas = new TCanvas("canvas");
	    zMass->Draw("e");
	    fun.Draw("same");
	    string epsFilename = "ZMassFitCoBwInGafGG_" + v_eps[i];
	    canvas->SaveAs(epsFilename.c_str());
	    canvas->SetLogy();
	    string epsLogFilename = "ZMassFitCoBwInGafGG_Log_" + v_eps[i];
	    canvas->SaveAs(epsLogFilename.c_str());
	  }
	}

      cout << "It works!\n";
  }
  catch(std::exception& e) {
    cerr << "error: " << e.what() << "\n";
    return 1;
  }
  catch(...) {
    cerr << "Exception of unknown type!\n";
  }
  return 0;
}


#include <sparse.h>
#include <sparse_fill.h>

#include <narrow.h>

#include <prox_rigid_body.h>
#include <prox_get_position_vector.h>
#include <prox_set_position_vector.h>

#include <prox_math_policy.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>

typedef prox::MathPolicy<float>      math_policy;
typedef math_policy::tiny_types      tiny_types;
typedef math_policy::real_type       real_type;
typedef math_policy::vector3_type    vector3_type;
typedef math_policy::vector7_type    vector7_type;
typedef math_policy::matrix3x3_type  matrix3x3_type;

BOOST_AUTO_TEST_SUITE(position_vector);
BOOST_AUTO_TEST_CASE(get_position_vector_test)
{
  // Initialize geometry
  narrow::System<tiny_types> narrow;

  size_t gid = narrow.create_geometry();

  // Initialize objects
  std::vector< prox::RigidBody< math_policy > > bodies;
  bodies.resize( 4u );

  bodies[0].set_geometry_idx( gid );
  bodies[0].set_position( vector3_type::make( 5.0,7.0,0.0) );

  bodies[1].set_geometry_idx( gid );
  bodies[1].set_position( vector3_type::make( 0.0,1.0,0.0) );

  bodies[2].set_geometry_idx( gid );
  bodies[2].set_position( vector3_type::make( 0.0,3.0,0.0) );

  bodies[3].set_geometry_idx( gid );
  bodies[3].set_position( vector3_type::make( 2.0,5.0,0.0) );

  vector7_type q;

  prox::get_position_vector(bodies.begin(),bodies.end(),q, math_policy());

  BOOST_CHECK_EQUAL(q(0)(0), 5);
  BOOST_CHECK_EQUAL(q(0)(1), 7);
  BOOST_CHECK_EQUAL(q(0)(2), 0);

  BOOST_CHECK_EQUAL(q(1)(0), 0);
  BOOST_CHECK_EQUAL(q(1)(1), 1);
  BOOST_CHECK_EQUAL(q(1)(2), 0);

  BOOST_CHECK_EQUAL(q(2)(0), 0);
  BOOST_CHECK_EQUAL(q(2)(1), 3);
  BOOST_CHECK_EQUAL(q(2)(2), 0);

  BOOST_CHECK_EQUAL(q(3)(0), 2);
  BOOST_CHECK_EQUAL(q(3)(1), 5);
  BOOST_CHECK_EQUAL(q(3)(2), 0);

  bodies[2].set_position( vector3_type::make( 8.0,3.0,0.5) );

  prox::get_position_vector(bodies.begin(),bodies.end(),q, math_policy());

  BOOST_CHECK_EQUAL(q(0)(0), 5);
  BOOST_CHECK_EQUAL(q(0)(1), 7);
  BOOST_CHECK_EQUAL(q(0)(2), 0);

  BOOST_CHECK_EQUAL(q(1)(0), 0);
  BOOST_CHECK_EQUAL(q(1)(1), 1);
  BOOST_CHECK_EQUAL(q(1)(2), 0);

  BOOST_CHECK_EQUAL(q(2)(0), 8);
  BOOST_CHECK_EQUAL(q(2)(1), 3);
  BOOST_CHECK_EQUAL(q(2)(2), 0.5);

  BOOST_CHECK_EQUAL(q(3)(0), 2);
  BOOST_CHECK_EQUAL(q(3)(1), 5);
  BOOST_CHECK_EQUAL(q(3)(2), 0);
}

BOOST_AUTO_TEST_CASE(set_position_vector_test)
{
  // Initialize geometry
  narrow::System<tiny_types> narrow;
  size_t gid = narrow.create_geometry();

  // Initialize objects
  std::vector< prox::RigidBody< math_policy > > bodies;
  bodies.resize( 4u );

  bodies[0].set_geometry_idx( gid );
  bodies[1].set_geometry_idx( gid );
  bodies[2].set_geometry_idx( gid );
  bodies[3].set_geometry_idx( gid );

  vector7_type q(4);

  sparse::fill(q(0));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(q(1));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(q(2));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(q(3));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'

  prox::set_position_vector(bodies.begin(),bodies.end(),q, math_policy());

  BOOST_CHECK_EQUAL(bodies[0].get_position()(0), 0);
  BOOST_CHECK_EQUAL(bodies[0].get_position()(1), 1);
  BOOST_CHECK_EQUAL(bodies[0].get_position()(2), 2);

  BOOST_CHECK_EQUAL(bodies[1].get_position()(0), 0);
  BOOST_CHECK_EQUAL(bodies[1].get_position()(1), 1);
  BOOST_CHECK_EQUAL(bodies[1].get_position()(2), 2);

  BOOST_CHECK_EQUAL(bodies[2].get_position()(0), 0);
  BOOST_CHECK_EQUAL(bodies[2].get_position()(1), 1);
  BOOST_CHECK_EQUAL(bodies[2].get_position()(2), 2);

  BOOST_CHECK_EQUAL(bodies[3].get_position()(0), 0);
  BOOST_CHECK_EQUAL(bodies[3].get_position()(1), 1);
  BOOST_CHECK_EQUAL(bodies[3].get_position()(2), 2);

  sparse::fill(q(2),4);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(q(3),7);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'

  prox::set_position_vector(bodies.begin(),bodies.end(),q, math_policy());

  BOOST_CHECK_EQUAL(bodies[0].get_position()(0), 0);
  BOOST_CHECK_EQUAL(bodies[0].get_position()(1), 1);
  BOOST_CHECK_EQUAL(bodies[0].get_position()(2), 2);

  BOOST_CHECK_EQUAL(bodies[1].get_position()(0), 0);
  BOOST_CHECK_EQUAL(bodies[1].get_position()(1), 1);
  BOOST_CHECK_EQUAL(bodies[1].get_position()(2), 2);

  BOOST_CHECK_EQUAL(bodies[2].get_position()(0), 4);
  BOOST_CHECK_EQUAL(bodies[2].get_position()(1), 5);
  BOOST_CHECK_EQUAL(bodies[2].get_position()(2), 6);

  BOOST_CHECK_EQUAL(bodies[3].get_position()(0), 7);
  BOOST_CHECK_EQUAL(bodies[3].get_position()(1), 8);
  BOOST_CHECK_EQUAL(bodies[3].get_position()(2), 9);
}
BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <fstream>
#include <stdio.h>
#include <cstring>
#include <vector>
#include <string>
#include <iomanip>
#include <boost/tokenizer.hpp>

using namespace std;

int main(int argc, char const *argv[])
{
    if(argc != 4)
    {
        cout << "El formato debe ser: ./" << argv[0] << " fichero.txt 'codigo de la proteina' 'numero de normalizado' " << endl;
        return 1;
    }
    int j;
    double size;
    string file = argv[1], sequence, code = argv[2];
    float number = std::stof(argv[3]);

    std::vector<std::string> result;
    ofstream output("NORMALIZADO: " + code);
    ifstream input;
    output << fixed;
    output << setprecision(15);
    input.open(file);
    while(!input.eof())
    {
        getline(input, sequence);
        boost::tokenizer<boost::char_separator<char>> tokens(sequence, boost::char_separator<char>(" "));
        std::vector<std::string> result(tokens.begin(), tokens.end());
        for(int i=0; i<result.size(); i++){
            switch (i)
            {
            case 0:
                output << result[i] << " ";
                break;
            case 1:
                output << (stod(result[i])/number) << " ";
                break;
            case 2:
                output << result[i] << endl;
            }
        }
    }
    input.close();
    output.close();

    return 0;
}


#ifndef PYTHONIC_INCLUDE_NUMPY_FMIN_HPP
#define PYTHONIC_INCLUDE_NUMPY_FMIN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/types/numpy_broadcast.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/min.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME fmin
#define NUMPY_NARY_FUNC_SYM boost::simd::min
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


// Copyright 2010-2014, D. E. Shaw Research.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt )

#ifndef BOOST_RANDOM_THREEFRY_HPP
#define BOOST_RANDOM_THREEFRY_HPP

#include <boost/array.hpp>
#include <boost/static_assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/rotl.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/for_each.hpp>

namespace boost{
namespace random{

// threefry_constants is an abstract template that will be
// specialized with the KS_PARITY and Rotation constants
// of the threefry generators.  These constants are carefully
// chosen to achieve good randomization.
//  threefry_constants<2, uint32_t>
//  threefry_constants<2, uint64_t>
//  threefry_constants<4, uint32_t>
//  threefry_constants<4, uint64_t>
// The constants here are from Salmon et al <FIXME REF>.
//
// See Salmon et al, or Schneier's original work on Threefish <FIXME
// REF> for information about how the constants were obtained.
template <unsigned _N, typename Uint>
struct threefry_constants{
};

// 2x32 constants
template <>
struct threefry_constants<2, uint32_t>{
    static const uint32_t KS_PARITY = UINT32_C(0x1BD11BDA);
    static const unsigned Rotations[8];
};
const unsigned
threefry_constants<2, uint32_t>::Rotations[]  =
    {13, 15, 26, 6, 17, 29, 16, 24};

// 4x32 contants
template <>
struct threefry_constants<4, uint32_t>{
    static const uint32_t KS_PARITY = UINT32_C(0x1BD11BDA);
    static const unsigned Rotations0[8];
    static const unsigned Rotations1[8];
};
const unsigned
threefry_constants<4, uint32_t>::Rotations0[]  =
    {10, 11, 13, 23, 6, 17, 25, 18};

const unsigned
threefry_constants<4, uint32_t>::Rotations1[]  =
    {26, 21, 27, 5, 20, 11, 10, 20};

// 2x64 constants
template <>
struct threefry_constants<2, uint64_t>{
    static const uint64_t KS_PARITY = UINT64_C(0x1BD11BDAA9FC1A22);
    static const unsigned Rotations[8];
};
const unsigned
threefry_constants<2, uint64_t>::Rotations[]  =
    {16, 42, 12, 31, 16, 32, 24, 21};

// 4x64 constants
template <>
struct threefry_constants<4, uint64_t>{
    static const uint64_t KS_PARITY = UINT64_C(0x1BD11BDAA9FC1A22);
    static const unsigned Rotations0[8];
    static const unsigned Rotations1[8];
};
const unsigned
threefry_constants<4, uint64_t>::Rotations0[]  =
    {14, 52, 23, 5, 25, 46, 58, 32};

const unsigned
threefry_constants<4, uint64_t>::Rotations1[]  = {
    16, 57, 40, 37, 33, 12, 22, 32};

template <unsigned N, typename Uint, unsigned R=20, typename Constants=threefry_constants<N, Uint> >
struct threefry{
    BOOST_STATIC_ASSERT( N==2 || N==4 );
    // should never be instantiated.
    // Only the specializations on N=2 and 4 are
    // permitted/implemented.
};

// specialize threefry<2, Uint, R>
template<typename Uint, unsigned R, typename Constants>
struct threefry<2, Uint, R, Constants>{
    typedef array<Uint, 2> domain_type;
    typedef array<Uint, 2> range_type;
    typedef array<Uint, 2> key_type ;

    threefry() : k(){}
    threefry(key_type _k) : k(_k) {}
    threefry(const threefry& v) : k(v.k){}

    void setkey(key_type _k){
        k = _k;
    }

    key_type getkey() const{
        return k;
    }

    bool operator==(const threefry& rhs) const{
        return k == rhs.k;
    }

    bool operator!=(const threefry& rhs) const{
        return k != rhs.k;
    }

    domain_type operator()(domain_type c){
        Uint ks[3];
        ks[2] = Constants::KS_PARITY;
        ks[0] = this->k[0]; ks[2] ^= this->k[0]; c[0] += this->k[0];
        ks[1] = this->k[1]; ks[2] ^= this->k[1]; c[1] += this->k[1];
#if 1   // gcc doesn't want to unroll this without some help from mpl::for_each
        _roundapplyer ra(c, ks);
        mpl::for_each<mpl::range_c<unsigned, 0, R> >( ra );
        return c;
#else
        for(unsigned r=0; r<R; ){
            c[0] += c[1]; c[1] = detail::rotl(c[1],Constants::Rotations[r%8]); c[1] ^= c[0];
            ++r;
            if((r&3)==0){
                unsigned r4 = r>>2;
                c[0] += ks[r4%3];
                c[1] += ks[(r4+1)%3] + r4;
            }
        }
        return c;
#endif
    }

protected:
    struct _roundapplyer{
        domain_type& c;
        Uint* ks;
        _roundapplyer(domain_type& _c, Uint* _ks) : c(_c), ks(_ks){}
        void operator()(unsigned r){
            c[0] += c[1]; c[1] = detail::rotl(c[1],Constants::Rotations[r%8]); c[1] ^= c[0];
            unsigned rplus = r+1;
            if((rplus&3)==0){
                unsigned r4 = rplus>>2;
                c[0] += ks[r4%3];
                c[1] += ks[(r4+1)%3] + r4;
            }
        }
    };
    key_type k;
};

// specialize threefry<4, Uint, R>
template<typename Uint, unsigned R, typename Constants>
struct threefry<4, Uint, R, Constants>{
    typedef array<Uint, 4> domain_type;
    typedef array<Uint, 4> range_type;
    typedef array<Uint, 4> key_type ;

    threefry() : k(){}
    threefry(key_type _k) : k(_k) {}
    threefry(const threefry& v) : k(v.k){}

    void setkey(key_type _k){
        k = _k;
    }

    key_type getkey() const{
        return k;
    }

    bool operator==(const threefry& rhs) const{
        return k == rhs.k;
    }

    bool operator!=(const threefry& rhs) const{
        return k != rhs.k;
    }

    range_type operator()(domain_type c){
        Uint ks[5];
        ks[4] = Constants::KS_PARITY;
        ks[0] = this->k[0]; ks[4] ^= this->k[0]; c[0] += this->k[0];
        ks[1] = this->k[1]; ks[4] ^= this->k[1]; c[1] += this->k[1];
        ks[2] = this->k[2]; ks[4] ^= this->k[2]; c[2] += this->k[2];
        ks[3] = this->k[3]; ks[4] ^= this->k[3]; c[3] += this->k[3];

#if 1   // gcc doesn't want to unroll this without some help from mpl::for_each
        _roundapplyer ra(c, ks);
        mpl::for_each<mpl::range_c<unsigned, 0, R> >( ra );
        return c;
#else
        for(unsigned r=0; r<R; ){
            if((r&1)==0){
                c[0] += c[1]; c[1] = detail::rotl(c[1],Constants::Rotations0[r%8]); c[1] ^= c[0];
                c[2] += c[3]; c[3] = detail::rotl(c[3],Constants::Rotations1[r%8]); c[3] ^= c[2];
            }else{
                c[0] += c[3]; c[3] = detail::rotl(c[3],Constants::Rotations0[r%8]); c[3] ^= c[0];
                c[2] += c[1]; c[1] = detail::rotl(c[1],Constants::Rotations1[r%8]); c[1] ^= c[2];
            }
            ++r;
            if((r&3)==0){
                unsigned r4 = r>>2;
                c[0] += ks[(r4+0)%5];
                c[1] += ks[(r4+1)%5];
                c[2] += ks[(r4+2)%5];
                c[3] += ks[(r4+3)%5] + r4;
            }
        }
        return c;
#endif
    }

protected:
    struct _roundapplyer{
        domain_type& c;
        Uint* ks;
        _roundapplyer(domain_type& _c, Uint* _ks) : c(_c), ks(_ks){}
        void operator()(unsigned r){
            if((r&1)==0){
                c[0] += c[1]; c[1] = detail::rotl(c[1],Constants::Rotations0[r%8]); c[1] ^= c[0];
                c[2] += c[3]; c[3] = detail::rotl(c[3],Constants::Rotations1[r%8]); c[3] ^= c[2];
            }else{
                c[0] += c[3]; c[3] = detail::rotl(c[3],Constants::Rotations0[r%8]); c[3] ^= c[0];
                c[2] += c[1]; c[1] = detail::rotl(c[1],Constants::Rotations1[r%8]); c[1] ^= c[2];
            }
            ++r;
            if((r&3)==0){
                unsigned r4 = r>>2;
                c[0] += ks[(r4+0)%5];
                c[1] += ks[(r4+1)%5];
                c[2] += ks[(r4+2)%5];
                c[3] += ks[(r4+3)%5] + r4;
            }
        }
    };
    key_type k;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_THREEFRY_HPP


#include <boost/random/xor_combine.hpp>