//---------------------------------------------------------------------------//
//!
//! \file   tstCoupledElasticDistribution.cpp
//! \author Luke Kersting
//! \brief  Coupled elastic distribution unit tests.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/io.hpp>

// FRENSIE Includes
#include "MonteCarlo_CoupledElasticDistribution.hpp"
#include "Utility_TabularUnivariateDistribution.hpp"
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_UnitTestHarnessWithMain.hpp"
#include "ArchiveTestHelpers.hpp"

//---------------------------------------------------------------------------//
// Testing Types
//---------------------------------------------------------------------------//
using boost::units::quantity;
using namespace Utility::Units;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

typedef quantity<si::dimensionless> dl;

typedef std::tuple<Utility::LinLin,Utility::LogLin,Utility::LinNudgedLogCos,Utility::LogNudgedLogCos > InterpTypes;

typedef TestArchiveHelper::TestArchives TestArchives;

typedef std::tuple<
  std::tuple<si::dimensionless,si::amount,cgs::dimensionless,si::amount>,
  std::tuple<cgs::dimensionless,si::amount,si::dimensionless,si::amount>,
  std::tuple<si::dimensionless,si::length,cgs::dimensionless,cgs::length>,
  std::tuple<cgs::dimensionless,cgs::length,si::dimensionless,si::length>,
  std::tuple<si::dimensionless,si::mass,cgs::dimensionless,cgs::mass>,
  std::tuple<cgs::dimensionless,cgs::mass,si::dimensionless,si::mass>,
  std::tuple<si::dimensionless,si::dimensionless,cgs::dimensionless,cgs::dimensionless>,
  std::tuple<cgs::dimensionless,cgs::dimensionless,si::dimensionless,si::dimensionless>,
  std::tuple<si::dimensionless,void*,cgs::dimensionless,void*>,
  std::tuple<cgs::dimensionless,void*,si::dimensionless,void*>
  > TestUnitTypeQuads;

//---------------------------------------------------------------------------//
// Testing Variables
//---------------------------------------------------------------------------//

std::shared_ptr<Utility::UnivariateDistribution> distribution;
std::shared_ptr<Utility::TabularUnivariateDistribution> tab_distribution;

std::shared_ptr<Utility::UnitAwareUnivariateDistribution<si::dimensionless,si::amount> >
  unit_aware_distribution;
std::shared_ptr<Utility::UnitAwareTabularUnivariateDistribution<si::dimensionless,si::amount> >
unit_aware_tab_distribution;

double eta = 1e-12;
double cross_section_ratio = 0.1;

//---------------------------------------------------------------------------//
// Testing Functions.
//---------------------------------------------------------------------------//
// Initialize the distribution
template<typename InterpolationPolicy, typename BaseDistribution>
void initialize( std::shared_ptr<BaseDistribution>& dist )
{
  // Use the basic constructor
  std::vector<typename BaseDistribution::IndepQuantity>
    independent_values( 4 );
  Utility::setQuantity( independent_values[0], -1.0 );
  Utility::setQuantity( independent_values[1], 0.0 );
  Utility::setQuantity( independent_values[2], 0.5 );
  Utility::setQuantity( independent_values[3], 0.999999 );

  std::vector<typename BaseDistribution::DepQuantity> dependent_values( 4 );
  Utility::setQuantity( dependent_values[0], 1e2 );
  Utility::setQuantity( dependent_values[1], 1e1 );
  Utility::setQuantity( dependent_values[2], 1.0 );
  Utility::setQuantity( dependent_values[3], 1e-1 );

  dist.reset(new MonteCarlo::UnitAwareCoupledElasticDistribution<InterpolationPolicy,typename BaseDistribution::IndepUnit, typename BaseDistribution::DepUnit>(
                                                      independent_values,
                                                      dependent_values,
                                                      eta,
                                                      cross_section_ratio ) );
}

//---------------------------------------------------------------------------//
// Tests.
//---------------------------------------------------------------------------//
// Check that the distribution can be evaluated
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            evaluate,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK_EQUAL( distribution->evaluate( -1.0 ), 1e1 );
  FRENSIE_CHECK_EQUAL( distribution->evaluate( 0.0 ), 1.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluate( 0.5 ), 1e-1 );
  FRENSIE_CHECK_FLOATING_EQUALITY( distribution->evaluate( 0.999999 ), 1e-2, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( distribution->evaluate( 1.0 ), 1.00000200000100021e+10, 1e-15 );

  double sample = distribution->evaluate( distribution->sample() );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be evaluated
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            evaluate,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( -1.0 ),
                       1e1*si::mole );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( 0.0 ),
                       1.0*si::mole );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( 0.5 ),
                       1e-1*si::mole );
  FRENSIE_CHECK_FLOATING_EQUALITY( unit_aware_distribution->evaluate( 0.999999 ),
                                  1e-2*si::mole,
                                  1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( unit_aware_distribution->evaluate( 1.0 ),
                                  1.00000200000100021e+10*si::mole,
                                  1e-15 );
}

//---------------------------------------------------------------------------//
// Check that the PDF can be evaluated
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            evaluatePDF,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK_FLOATING_EQUALITY( distribution->evaluatePDF( -1.0 ),
                          1.7233951046594968293e-1,
                          1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY( distribution->evaluatePDF( 0.0 ),
                          1.7233951046594968293e-2,
                          1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY( distribution->evaluatePDF( 0.5 ),
                          1.7233951046594968293e-3,
                          1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY( distribution->evaluatePDF( 0.999999 ),
                          1.7233951046594968293e-4,
                          1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY( distribution->evaluatePDF( 1.0 ),
                          1.7233985514514300972e9,
                          1e-6 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware PDF can be evaluated
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            evaluatePDF,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK_FLOATING_EQUALITY(
                             unit_aware_distribution->evaluatePDF( -1.0*si::dimensionless() ),
                             1.7233951046594968293e-1/si::dimensionless(),
                             1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                             unit_aware_distribution->evaluatePDF( 0.0*si::dimensionless() ),
                             1.7233951046594968293e-2/si::dimensionless(),
                             1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                             unit_aware_distribution->evaluatePDF( 0.5*si::dimensionless() ),
                             1.7233951046594968293e-3/si::dimensionless(),
                             1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                              unit_aware_distribution->evaluatePDF( 0.999999*si::dimensionless() ),
                              1.7233951046594968293e-4/si::dimensionless(),
                              1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                              unit_aware_distribution->evaluatePDF( 1.0*si::dimensionless() ),
                              1.7233985514514300972e9/si::dimensionless(),
                              1e-6 );
}

//---------------------------------------------------------------------------//
// Check that the CDF can be evaluated
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            evaluateCDF,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( tab_distribution );

  FRENSIE_CHECK_FLOATING_EQUALITY( tab_distribution->evaluateCDF( -1.0 ),
                          0.0000000000,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( tab_distribution->evaluateCDF( 0.0 ),
                          0.094786730756272336018,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( tab_distribution->evaluateCDF( 0.5 ),
                          0.09952606729408594588,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( tab_distribution->evaluateCDF( 0.999999 ),
                          0.1,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( tab_distribution->evaluateCDF( 1.0 ),
                          1.0000000000,
                          1e-10 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware CDF can be evaluated
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            evaluateCDF,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_tab_distribution );

  FRENSIE_CHECK_FLOATING_EQUALITY( unit_aware_tab_distribution->evaluateCDF( -1.0*si::dimensionless() ),
                          0.0000000000,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( unit_aware_tab_distribution->evaluateCDF( 0.0*si::dimensionless() ),
                          0.094786730756272336018,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( unit_aware_tab_distribution->evaluateCDF( 0.5*si::dimensionless() ),
                          0.09952606729408594588,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( unit_aware_tab_distribution->evaluateCDF( 0.999999*si::dimensionless() ),
                          0.1,
                          1e-10 );
  FRENSIE_CHECK_FLOATING_EQUALITY( unit_aware_tab_distribution->evaluateCDF( 1.0*si::dimensionless() ),
                          1.0,
                          1e-10 );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            sample,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = cross_section_ratio;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  double sample = distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, -1.0 );

  sample = distribution->sample();
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999, 1e-12 );

  sample = distribution->sample();
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0, 1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = distribution->sample();
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0 );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            sample,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = cross_section_ratio;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  quantity<si::dimensionless> sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, -1.0*si::dimensionless() );

  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999*si::dimensionless(), 1e-12 );

  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0*si::dimensionless(), 1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0*si::dimensionless() );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1.0*si::dimensionless() );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            sampleAndRecordTrials,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = cross_section_ratio;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  Utility::DistributionTraits::Counter trials = 0;

  double sample = distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, -1.0 );
  FRENSIE_CHECK_EQUAL( 1.0/trials, 1.0 );

  sample = distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999, 1e-12 );
  FRENSIE_CHECK_EQUAL( 2.0/trials, 1.0 );

  sample = distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0, 1e-12 );
  FRENSIE_CHECK_EQUAL( 3.0/trials, 1.0 );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0 );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1.0 );
  FRENSIE_CHECK_EQUAL( 4.0/trials, 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            sampleAndRecordTrials,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = cross_section_ratio;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  Utility::DistributionTraits::Counter trials = 0;

  quantity<si::dimensionless> sample =
    unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, -1.0*si::dimensionless() );
  FRENSIE_CHECK_EQUAL( 1.0/trials, 1.0 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999*si::dimensionless(), 1e-12 );
  FRENSIE_CHECK_EQUAL( 2.0/trials, 1.0 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0*si::dimensionless(), 1e-12 );
  FRENSIE_CHECK_EQUAL( 3.0/trials, 1.0 );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0*si::dimensionless() );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1.0*si::dimensionless() );
  FRENSIE_CHECK_EQUAL( 4.0/trials, 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            sampleAndRecordBinIndex,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( tab_distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = cross_section_ratio;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  size_t bin_index;

  double sample = tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_EQUAL( sample, -1.0 );
  FRENSIE_CHECK_EQUAL( bin_index, 0u );

  sample = tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999, 1e-12 );
  FRENSIE_CHECK_EQUAL( bin_index, 3u );

  sample = tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0, 1e-12 );
  FRENSIE_CHECK_EQUAL( bin_index, 3u );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0 );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            sampleAndRecordBinIndex,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_tab_distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = cross_section_ratio;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  size_t bin_index;

  quantity<si::dimensionless> sample =
    unit_aware_tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_EQUAL( sample, -1.0*si::dimensionless() );
  FRENSIE_CHECK_EQUAL( bin_index, 0u );

  sample = unit_aware_tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999*si::dimensionless(), 1e-12 );
  FRENSIE_CHECK_EQUAL( bin_index, 3u );

  sample = unit_aware_tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0*si::dimensionless(), 1e-12 );
  FRENSIE_CHECK_EQUAL( bin_index, 3u );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = unit_aware_tab_distribution->sampleAndRecordBinIndex( bin_index );
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0*si::dimensionless() );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1.0*si::dimensionless() );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            sampleWithRandomNumber,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( tab_distribution );

  double sample = tab_distribution->sampleWithRandomNumber( 0.0 );
  FRENSIE_CHECK_EQUAL( sample, -1.0 );

  sample = tab_distribution->sampleWithRandomNumber( cross_section_ratio );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999, 1e-12 );

  sample = tab_distribution->sampleWithRandomNumber( 1.0 - 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0, 1e-12 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            sampleWithRandomNumber,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_tab_distribution );

  quantity<si::dimensionless> sample =
    unit_aware_tab_distribution->sampleWithRandomNumber( 0.0 );
  FRENSIE_CHECK_EQUAL( sample, -1.0*si::dimensionless() );

  sample = unit_aware_tab_distribution->sampleWithRandomNumber( cross_section_ratio );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 0.999999*si::dimensionless(), 1e-12 );

  sample = unit_aware_tab_distribution->sampleWithRandomNumber( 1.0 - 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1.0*si::dimensionless(), 1e-12 );

  double random_number = cross_section_ratio/2.0;
  sample = unit_aware_tab_distribution->sampleWithRandomNumber( random_number );
  FRENSIE_CHECK_GREATER( sample.value(), -1.0 );
  FRENSIE_CHECK_LESS( sample.value(), 0.999999 );

  random_number = (1.0 - cross_section_ratio)/2.0;
  sample = unit_aware_tab_distribution->sampleWithRandomNumber( random_number );
  FRENSIE_CHECK_GREATER( sample.value(), 0.999999 );
  FRENSIE_CHECK_LESS( sample.value(), 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled from a subrange
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            sampleInSubrange,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( tab_distribution );

  std::vector<double> fake_stream( 2 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  double sample = tab_distribution->sampleInSubrange( 1e-1  );
  FRENSIE_CHECK_EQUAL( sample, -1.0 );

  sample = tab_distribution->sampleInSubrange( 1e-1 );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1e-1, 1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = tab_distribution->sampleInSubrange( 1e-1 );
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0 );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1e-1 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled from a subrange
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            sampleInSubrange,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_tab_distribution );

  std::vector<double> fake_stream( 2 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  quantity<si::dimensionless> sample =
    unit_aware_tab_distribution->sampleInSubrange( 1e-1*si::dimensionless()  );
  FRENSIE_CHECK_EQUAL( sample, -1.0*si::dimensionless() );

  sample = unit_aware_tab_distribution->sampleInSubrange( 1e-1*si::dimensionless() );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1e-1*si::dimensionless(), 1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
  Utility::RandomNumberGenerator::initialize();

  sample = unit_aware_tab_distribution->sampleInSubrange( 1e-1*si::dimensionless() );
  FRENSIE_CHECK_GREATER_OR_EQUAL( sample, -1.0*si::dimensionless() );
  FRENSIE_CHECK_LESS_OR_EQUAL( sample, 1e-1*si::dimensionless() );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled from a subrange
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            sampleWithRandomNumberInSubrange,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( tab_distribution );

  double sample =
    tab_distribution->sampleWithRandomNumberInSubrange( 0.0, 1e-1  );
  FRENSIE_CHECK_EQUAL( sample, -1.0 );

  sample = tab_distribution->sampleWithRandomNumberInSubrange( 1.0, 1e-1 );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1e-1, 1e-12 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled from a subrange
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            sampleWithRandomNumberInSubrange,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_tab_distribution );

  quantity<si::dimensionless> sample =
    unit_aware_tab_distribution->sampleWithRandomNumberInSubrange(
                                                               0.0, 1e-1*si::dimensionless() );
  FRENSIE_CHECK_EQUAL( sample, -1.0*si::dimensionless() );

  sample = unit_aware_tab_distribution->sampleWithRandomNumberInSubrange(
                                                               1.0, 1e-1*si::dimensionless() );
  FRENSIE_CHECK_FLOATING_EQUALITY( sample, 1e-1*si::dimensionless(), 1e-12 );
}

//---------------------------------------------------------------------------//
// Check that the upper bound of the distribution independent variable can be
// returned
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            getUpperBoundOfIndepVar,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK_EQUAL( distribution->getUpperBoundOfIndepVar(), 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the upper bound of the unit-aware distribution independent
// variable can be returned
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            getUpperBoundOfIndepVar,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK_EQUAL( unit_aware_distribution->getUpperBoundOfIndepVar(),
                       1.0*si::dimensionless() );
}

//---------------------------------------------------------------------------//
// Check that the cutoff bound of the distribution independent variable can be
// returned
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            getCutoffBoundOfIndepVar,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  std::shared_ptr<MonteCarlo::CoupledElasticDistribution<InterpolationPolicy> >
                coupled_distribution;

  initialize<InterpolationPolicy>( coupled_distribution );

  FRENSIE_CHECK_EQUAL( coupled_distribution->getCutoffBoundOfIndepVar(), 0.999999 );
}

//---------------------------------------------------------------------------//
// Check that the cutoff bound of the unit-aware distribution independent
// variable can be returned
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            getCutoffBoundOfIndepVar,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

std::shared_ptr<MonteCarlo::UnitAwareCoupledElasticDistribution<InterpolationPolicy,si::dimensionless,si::amount> >
unit_aware_coupled_distribution;

  initialize<InterpolationPolicy>( unit_aware_coupled_distribution );

  FRENSIE_CHECK_EQUAL( unit_aware_coupled_distribution->getCutoffBoundOfIndepVar(),
                       0.999999*si::dimensionless() );
}

//---------------------------------------------------------------------------//
// Check that the lower bound of the distribution independent variable can be
// returned
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            getLowerBoundOfIndepVar,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK_EQUAL( distribution->getLowerBoundOfIndepVar(), -1.0 );
}

//---------------------------------------------------------------------------//
// Check that the lower bound of the unit-aware distribution independent
// variable can be returned
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            getLowerBoundOfIndepVar,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK_EQUAL( unit_aware_distribution->getLowerBoundOfIndepVar(),
                       -1.0*si::dimensionless() );
}

//---------------------------------------------------------------------------//
// Check that the distribution type can be returned
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            getDistributionType,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK_EQUAL( distribution->getDistributionType(),
                       Utility::COUPLED_ELASTIC_DISTRIBUTION );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution type can be returned
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            getDistributionType,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK_EQUAL( unit_aware_distribution->getDistributionType(),
                       Utility::COUPLED_ELASTIC_DISTRIBUTION );
}

//---------------------------------------------------------------------------//
// Check if the distribution is tabular
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            isTabular,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK( distribution->isTabular() );
}

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is tabular
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            isTabular,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK( unit_aware_distribution->isTabular() );
}

//---------------------------------------------------------------------------//
// Check that the distribution is continuous
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            isContinuous,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK( distribution->isContinuous() );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution is continuous
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            isContinuous,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK( unit_aware_distribution->isContinuous() );
}

//---------------------------------------------------------------------------//
// Check if the distribution is compatible with the interpolation type
FRENSIE_UNIT_TEST_TEMPLATE( CoupledElasticDistribution,
                            isCompatibleWithInterpType,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( distribution );

  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LogNudgedLogCos >() );
  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LinNudgedLogCos >() );
  FRENSIE_CHECK( !distribution->isCompatibleWithInterpType<Utility::LogLog>() );
  FRENSIE_CHECK( !distribution->isCompatibleWithInterpType<Utility::LinLog>() );

  // if( boost::is_same<InterpolationPolicy,Utility::LinLin>::value )
  // {
  //   FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  // }

  // if( boost::is_same<InterpolationPolicy,Utility::LinLog>::value )
  // {
  //   FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LinLog>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !distribution->isCompatibleWithInterpType<Utility::LinLog>() );
  // }

  // if( boost::is_same<InterpolationPolicy,Utility::LogLin>::value )
  // {
  //   FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  // }

  // if( boost::is_same<InterpolationPolicy,Utility::LogLog>::value )
  // {
  //   FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LogLog>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !distribution->isCompatibleWithInterpType<Utility::LogLog>() );
  // }
}

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is compatible with the interp type
FRENSIE_UNIT_TEST_TEMPLATE( UnitAwareCoupledElasticDistribution,
                            isCompatibleWithInterpType,
                            InterpTypes )
{
  FETCH_TEMPLATE_PARAM( 0, InterpolationPolicy );

  initialize<InterpolationPolicy>( unit_aware_distribution );

  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LogNudgedLogCos >() );
  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LinNudgedLogCos >() );
  FRENSIE_CHECK( !unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLog>() );
  FRENSIE_CHECK( !unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLog>() );

  // if( boost::is_same<InterpolationPolicy,Utility::LinLin>::value )
  // {
  //   FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  // }

  // if( boost::is_same<InterpolationPolicy,Utility::LinLog>::value )
  // {
  //   FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLog>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLog>() );
  // }

  // if( boost::is_same<InterpolationPolicy,Utility::LogLin>::value )
  // {
  //   FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  // }

  // if( boost::is_same<InterpolationPolicy,Utility::LogLog>::value )
  // {
  //   FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLog>() );
  // }
  // else
  // {
  //   FRENSIE_CHECK( !unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLog>() );
  // }
}

//---------------------------------------------------------------------------//
// Check that distributions can be scaled
FRENSIE_UNIT_TEST_TEMPLATE_EXPAND( UnitAwareCoupledElasticDistribution,
                                   explicit_conversion,
                                   TestUnitTypeQuads )
{
  FETCH_TEMPLATE_PARAM( 0, RawIndepUnitA );
  FETCH_TEMPLATE_PARAM( 1, RawDepUnitA );
  FETCH_TEMPLATE_PARAM( 2, RawIndepUnitB );
  FETCH_TEMPLATE_PARAM( 3, RawDepUnitB );

  typedef typename std::remove_pointer<RawIndepUnitA>::type IndepUnitA;
  typedef typename std::remove_pointer<RawDepUnitA>::type DepUnitA;
  typedef typename std::remove_pointer<RawIndepUnitB>::type IndepUnitB;
  typedef typename std::remove_pointer<RawDepUnitB>::type DepUnitB;

  typedef typename Utility::UnitTraits<IndepUnitA>::template GetQuantityType<double>::type IndepQuantityA;
  typedef typename Utility::UnitTraits<typename Utility::UnitTraits<IndepUnitA>::InverseUnit>::template GetQuantityType<double>::type InverseIndepQuantityA;

  typedef typename Utility::UnitTraits<IndepUnitB>::template GetQuantityType<double>::type IndepQuantityB;
  typedef typename Utility::UnitTraits<typename Utility::UnitTraits<IndepUnitB>::InverseUnit>::template GetQuantityType<double>::type InverseIndepQuantityB;

  typedef typename Utility::UnitTraits<DepUnitA>::template GetQuantityType<double>::type DepQuantityA;
  typedef typename Utility::UnitTraits<DepUnitB>::template GetQuantityType<double>::type DepQuantityB;

  initialize<Utility::LinLin>( distribution );

  // Copy from unitless distribution to distribution type A
  MonteCarlo::UnitAwareCoupledElasticDistribution<Utility::LinLin,IndepUnitA,DepUnitA>
    unit_aware_dist_a_copy = MonteCarlo::UnitAwareCoupledElasticDistribution<Utility::LinLin,IndepUnitA,DepUnitA>::fromUnitlessDistribution( *dynamic_cast<MonteCarlo::CoupledElasticDistribution<Utility::LinLin>*>( distribution.get() ) );

  // Copy from distribution type A to distribution type B
  MonteCarlo::UnitAwareCoupledElasticDistribution<Utility::LinLin,IndepUnitB,DepUnitB>
    unit_aware_dist_b_copy( unit_aware_dist_a_copy );

  IndepQuantityA indep_quantity_a =
    Utility::QuantityTraits<IndepQuantityA>::initializeQuantity( 0.0 );
  InverseIndepQuantityA inv_indep_quantity_a =
    Utility::QuantityTraits<InverseIndepQuantityA>::initializeQuantity( 0.017233951046594968293 );
  DepQuantityA dep_quantity_a =
    Utility::QuantityTraits<DepQuantityA>::initializeQuantity( 1.0 );

  IndepQuantityB indep_quantity_b( indep_quantity_a );
  InverseIndepQuantityB inv_indep_quantity_b( inv_indep_quantity_a );
  DepQuantityB dep_quantity_b( dep_quantity_a );

  FRENSIE_CHECK_FLOATING_EQUALITY(
                           unit_aware_dist_a_copy.evaluate( indep_quantity_a ),
                           dep_quantity_a,
                           1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                        unit_aware_dist_a_copy.evaluatePDF( indep_quantity_a ),
                        inv_indep_quantity_a,
                        1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                           unit_aware_dist_b_copy.evaluate( indep_quantity_b ),
                           dep_quantity_b,
                           1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                        unit_aware_dist_b_copy.evaluatePDF( indep_quantity_b ),
                        inv_indep_quantity_b,
                        1e-15 );

  Utility::setQuantity( indep_quantity_a, 0.5 );
  Utility::setQuantity( inv_indep_quantity_a, 0.0017233951046594968293 );
  Utility::setQuantity( dep_quantity_a, 0.1 );

  indep_quantity_b = IndepQuantityB( indep_quantity_a );
  inv_indep_quantity_b = InverseIndepQuantityB( inv_indep_quantity_a );
  dep_quantity_b = DepQuantityB( dep_quantity_a );

  FRENSIE_CHECK_FLOATING_EQUALITY(
                           unit_aware_dist_a_copy.evaluate( indep_quantity_a ),
                           dep_quantity_a,
                           1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                        unit_aware_dist_a_copy.evaluatePDF( indep_quantity_a ),
                        inv_indep_quantity_a,
                        1e-6 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                           unit_aware_dist_b_copy.evaluate( indep_quantity_b ),
                           dep_quantity_b,
                           1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY(
                        unit_aware_dist_b_copy.evaluatePDF( indep_quantity_b ),
                        inv_indep_quantity_b,
                        1e-6 );
}

//---------------------------------------------------------------------------//
// Check that a distribution can be archived
FRENSIE_UNIT_TEST_TEMPLATE_EXPAND( CoupledElasticDistribution,
                                   archive,
                                   TestArchives )
{
  FETCH_TEMPLATE_PARAM( 0, RawOArchive );
  FETCH_TEMPLATE_PARAM( 1, RawIArchive );

  typedef typename std::remove_pointer<RawOArchive>::type OArchive;
  typedef typename std::remove_pointer<RawIArchive>::type IArchive;

  std::string archive_base_name( "test_coupled_elastic_dist" );
  std::ostringstream archive_ostream;

  // Create and archive some uniform distributions
  {
    std::unique_ptr<OArchive> oarchive;

    createOArchive( archive_base_name, archive_ostream, oarchive );

    std::shared_ptr<MonteCarlo::CoupledElasticDistribution<Utility::LinLin> >
      concrete_dist;
    initialize<Utility::LinLin>( concrete_dist );

    std::shared_ptr<Utility::TabularUnivariateDistribution>
      intermediate_base_dist;

    initialize<Utility::LogLin>( intermediate_base_dist );

    std::shared_ptr<Utility::UnivariateDistribution>
      base_dist_a;

    initialize<Utility::LinNudgedLogCos >( base_dist_a );

    std::shared_ptr<Utility::UnivariateDistribution>
      base_dist_b;

    initialize<Utility::LogNudgedLogCos >( base_dist_b );

    FRENSIE_REQUIRE_NO_THROW( (*oarchive) << boost::serialization::make_nvp( "concrete_dist", *concrete_dist ) );
    FRENSIE_REQUIRE_NO_THROW( (*oarchive) << BOOST_SERIALIZATION_NVP( intermediate_base_dist ) );
    FRENSIE_REQUIRE_NO_THROW( (*oarchive) << BOOST_SERIALIZATION_NVP( base_dist_a ) );
    FRENSIE_REQUIRE_NO_THROW( (*oarchive) << BOOST_SERIALIZATION_NVP( base_dist_b ) );
  }

  // Copy the archive ostream to an istream
  std::istringstream archive_istream( archive_ostream.str() );

  // Load the archived distributions
  std::unique_ptr<IArchive> iarchive;

  createIArchive( archive_istream, iarchive );

  MonteCarlo::CoupledElasticDistribution<Utility::LinLin> concrete_dist;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( concrete_dist ) );

  FRENSIE_CHECK_EQUAL( concrete_dist.evaluate( -1.0 ), 1e1 );
  FRENSIE_CHECK_EQUAL( concrete_dist.evaluate( 0.0 ), 1.0 );
  FRENSIE_CHECK_EQUAL( concrete_dist.evaluate( 0.5 ), 1e-1 );
  FRENSIE_CHECK_FLOATING_EQUALITY( concrete_dist.evaluate( 0.999999 ), 1e-2, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( concrete_dist.evaluate( 1.0 ), 1.00000200000100021e+10, 1e-15 );

  std::shared_ptr<Utility::TabularUnivariateDistribution>
    intermediate_base_dist;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( intermediate_base_dist ) );

  FRENSIE_CHECK_EQUAL( intermediate_base_dist->evaluate( -1.0 ), 1e1 );
  FRENSIE_CHECK_EQUAL( intermediate_base_dist->evaluate( 0.0 ), 1.0 );
  FRENSIE_CHECK_EQUAL( intermediate_base_dist->evaluate( 0.5 ), 1e-1 );
  FRENSIE_CHECK_FLOATING_EQUALITY( intermediate_base_dist->evaluate( 0.999999 ), 1e-2, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( intermediate_base_dist->evaluate( 1.0 ), 1.00000200000100021e+10, 1e-15 );

  std::shared_ptr<Utility::UnivariateDistribution> base_dist_a;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( base_dist_a ) );

  FRENSIE_CHECK_EQUAL( base_dist_a->evaluate( -1.0 ), 1e1 );
  FRENSIE_CHECK_EQUAL( base_dist_a->evaluate( 0.0 ), 1.0 );
  FRENSIE_CHECK_EQUAL( base_dist_a->evaluate( 0.5 ), 1e-1 );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_a->evaluate( 0.999999 ), 1e-2, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_a->evaluate( 1.0 ), 1.00000200000100021e+10, 1e-15 );

  std::shared_ptr<Utility::UnivariateDistribution> base_dist_b;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( base_dist_b ) );

  FRENSIE_CHECK_EQUAL( base_dist_b->evaluate( -1.0 ), 1e1 );
  FRENSIE_CHECK_EQUAL( base_dist_b->evaluate( 0.0 ), 1.0 );
  FRENSIE_CHECK_EQUAL( base_dist_b->evaluate( 0.5 ), 1e-1 );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_b->evaluate( 0.999999 ), 1e-2, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_b->evaluate( 1.0 ), 1.00000200000100021e+10, 1e-15 );
}

//---------------------------------------------------------------------------//
// Check that a distribution can be archived
FRENSIE_UNIT_TEST_TEMPLATE_EXPAND( UnitAwareCoupledElasticDistribution,
                                   archive,
                                   TestArchives )
{
  FETCH_TEMPLATE_PARAM( 0, RawOArchive );
  FETCH_TEMPLATE_PARAM( 1, RawIArchive );

  typedef typename std::remove_pointer<RawOArchive>::type OArchive;
  typedef typename std::remove_pointer<RawIArchive>::type IArchive;

  std::string archive_base_name( "test_coupled_elastic_dist" );
  std::ostringstream archive_ostream;

  // Create and archive some uniform distributions
  {
    std::unique_ptr<OArchive> oarchive;

    createOArchive( archive_base_name, archive_ostream, oarchive );

    std::shared_ptr<MonteCarlo::UnitAwareCoupledElasticDistribution<Utility::LinLin,si::dimensionless,si::amount> >
      concrete_dist;
    initialize<Utility::LinLin>( concrete_dist );

    std::shared_ptr<Utility::UnitAwareTabularUnivariateDistribution<si::dimensionless,si::amount> >
      intermediate_base_dist;

    initialize<Utility::LogLin>( intermediate_base_dist );

    std::shared_ptr<Utility::UnitAwareUnivariateDistribution<si::dimensionless,si::amount> >
      base_dist_a;

    initialize<Utility::LinNudgedLogCos >( base_dist_a );

    std::shared_ptr<Utility::UnitAwareUnivariateDistribution<si::dimensionless,si::amount> >
      base_dist_b;

    initialize<Utility::LogNudgedLogCos >( base_dist_b );

    FRENSIE_CHECK_NO_THROW( (*oarchive) << boost::serialization::make_nvp( "concrete_dist", *concrete_dist ) );
    FRENSIE_CHECK_NO_THROW( (*oarchive) << BOOST_SERIALIZATION_NVP( intermediate_base_dist ) );
    FRENSIE_CHECK_NO_THROW( (*oarchive) << BOOST_SERIALIZATION_NVP( base_dist_a ) );
    FRENSIE_CHECK_NO_THROW( (*oarchive) << BOOST_SERIALIZATION_NVP( base_dist_b ) );
  }

  // Copy the archive ostream to an istream
  std::istringstream archive_istream( archive_ostream.str() );

  // Load the archived distributions
  std::unique_ptr<IArchive> iarchive;

  createIArchive( archive_istream, iarchive );

  MonteCarlo::UnitAwareCoupledElasticDistribution<Utility::LinLin,si::dimensionless,si::amount> concrete_dist;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( concrete_dist ) );

  FRENSIE_CHECK_EQUAL( concrete_dist.evaluate( -1.0 ), 1e1*si::mole );
  FRENSIE_CHECK_EQUAL( concrete_dist.evaluate( 0.0 ), 1.0*si::mole );
  FRENSIE_CHECK_EQUAL( concrete_dist.evaluate( 0.5 ), 1e-1*si::mole );
  FRENSIE_CHECK_FLOATING_EQUALITY( concrete_dist.evaluate( 0.999999 ), 1e-2*si::mole, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( concrete_dist.evaluate( 1.0 ), 1.00000200000100021e+10*si::mole, 1e-15 );

  std::shared_ptr<Utility::UnitAwareTabularUnivariateDistribution<si::dimensionless,si::amount> >
    intermediate_base_dist;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( intermediate_base_dist ) );

  FRENSIE_CHECK_EQUAL( intermediate_base_dist->evaluate( -1.0 ), 1e1*si::mole );
  FRENSIE_CHECK_EQUAL( intermediate_base_dist->evaluate( 0.0 ), 1.0*si::mole );
  FRENSIE_CHECK_EQUAL( intermediate_base_dist->evaluate( 0.5 ), 1e-1*si::mole );
  FRENSIE_CHECK_FLOATING_EQUALITY( intermediate_base_dist->evaluate( 0.999999 ), 1e-2*si::mole, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( intermediate_base_dist->evaluate( 1.0 ), 1.00000200000100021e+10*si::mole, 1e-15 );

  std::shared_ptr<Utility::UnitAwareUnivariateDistribution<si::dimensionless,si::amount> > base_dist_a;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( base_dist_a ) );

  FRENSIE_CHECK_EQUAL( base_dist_a->evaluate( -1.0 ), 1e1*si::mole );
  FRENSIE_CHECK_EQUAL( base_dist_a->evaluate( 0.0 ), 1.0*si::mole );
  FRENSIE_CHECK_EQUAL( base_dist_a->evaluate( 0.5 ), 1e-1*si::mole );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_a->evaluate( 0.999999 ), 1e-2*si::mole, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_a->evaluate( 1.0 ), 1.00000200000100021e+10*si::mole, 1e-15 );

  std::shared_ptr<Utility::UnitAwareUnivariateDistribution<si::dimensionless,si::amount> > base_dist_b;

  FRENSIE_REQUIRE_NO_THROW( (*iarchive) >> BOOST_SERIALIZATION_NVP( base_dist_b ) );

  FRENSIE_CHECK_EQUAL( base_dist_b->evaluate( -1.0 ), 1e1*si::mole );
  FRENSIE_CHECK_EQUAL( base_dist_b->evaluate( 0.0 ), 1.0*si::mole );
  FRENSIE_CHECK_EQUAL( base_dist_b->evaluate( 0.5 ), 1e-1*si::mole );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_b->evaluate( 0.999999 ), 1e-2*si::mole, 1e-15 );
  FRENSIE_CHECK_FLOATING_EQUALITY( base_dist_b->evaluate( 1.0 ), 1.00000200000100021e+10*si::mole, 1e-15 );
}

//---------------------------------------------------------------------------//
// Custom setup
//---------------------------------------------------------------------------//
FRENSIE_CUSTOM_UNIT_TEST_SETUP_BEGIN();

FRENSIE_CUSTOM_UNIT_TEST_INIT()
{
  // Initialize the random number generator
  Utility::RandomNumberGenerator::createStreams();
}

FRENSIE_CUSTOM_UNIT_TEST_SETUP_END();

//---------------------------------------------------------------------------//
// end tstCoupledElasticDistribution.cpp
//---------------------------------------------------------------------------//


#include "RooRealVar.h"
#include "RooRealConstant.h"
#include "RooGaussian.h"
#include "RooExponential.h"
#include "RooAddPdf.h"
#include "RooAddition.h"
#include "RooDataSet.h"
#include "RooDataHist.h"
#include "RooHistPdf.h"
#include "RooChebychev.h"
#include "RooExponential.h"
#include "RooProdPdf.h"
#include "RooChi2Var.h"
#include "RooGlobalFunc.h"
#include "RooPlot.h"
#include "RooMinuit.h"
#include "RooFitResult.h"
#include "RooFormulaVar.h"
#include "RooGenericPdf.h"
#include "RooExtendPdf.h"
#include "TCanvas.h"
#include "TROOT.h"
#include "TFile.h"
#include "TH1.h"
#include "RooNLLVar.h"
#include "RooRandom.h"
#include "TRandom3.h"
#include <iostream>
#include <fstream>
#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;
#include <vector>

using namespace std;
using namespace RooFit;

// A function that get histogram, restricting it on fit range  and sets contents to 0.0 if entries are too small

TH1F *getHisto(TFile *file, const char *name, double fMin, double fMax, unsigned int rebin) {
  TObject *h = file->Get(name);
  if (h == nullptr)
    cout << "Can't find object " << name << "\n";
  TH1F *histo = dynamic_cast<TH1F *>(h);
  if (histo == nullptr)
    cout << "Object " << name << " is of type " << h->ClassName() << ", not TH1\n";
  TH1F *new_histo = new TH1F(name, name, (int)(fMax - fMin), fMin, fMax);
  int bin_num = 0;
  for (int i = (int)fMin; i <= (int)fMax; ++i) {
    bin_num = (i - (int)fMin + 1);
    new_histo->SetBinContent(bin_num, histo->GetBinContent(i));
  }
  delete histo;
  new_histo->Sumw2();
  new_histo->Rebin(rebin);
  for (int i = 1; i <= new_histo->GetNbinsX(); ++i) {
    if (new_histo->GetBinContent(i) == 0.00) {
      cout << " WARNING: histo " << name << " has 0 enter in bin number " << i << endl;
    }
    if (new_histo->GetBinContent(i) < 0.1) {
      new_histo->SetBinContent(i, 0.0);
      new_histo->SetBinError(i, 0.0);
      cout << " WARNING: setting value 0.0 to histo " << name << " for bin number " << i << endl;
    }
  }

  return new_histo;
}

// a function that create fromm a model pdf a toy RooDataHist

RooDataHist *genHistFromModelPdf(
    const char *name, RooAbsPdf *model, RooRealVar *var, double ScaleLumi, int range, int rebin, int seed) {
  double genEvents = model->expectedEvents(*var);
  TRandom3 *rndm = new TRandom3();
  rndm->SetSeed(seed);
  double nEvt = rndm->PoissonD(genEvents);
  int intEvt = ((nEvt - (int)nEvt) >= 0.5) ? (int)nEvt + 1 : int(nEvt);
  RooDataSet *data = model->generate(*var, intEvt);
  cout << " expected events for " << name << " = " << genEvents << endl;
  cout << " data->numEntries() for name " << name << " == " << data->numEntries() << endl;
  // cout<< " nEvt from PoissonD for" << name << " == " << nEvt<< endl;
  //cout<< " cast of nEvt  for" << name << " == " << intEvt<< endl;
  RooAbsData *binned_data = data->binnedClone();
  TH1 *toy_hist = binned_data->createHistogram(name, *var, Binning(range / rebin));
  for (int i = 1; i <= toy_hist->GetNbinsX(); ++i) {
    toy_hist->SetBinError(i, sqrt(toy_hist->GetBinContent(i)));
    if (toy_hist->GetBinContent(i) == 0.00) {
      cout << " WARNING: histo " << name << " has 0 enter in bin number " << i << endl;
    }
    if (toy_hist->GetBinContent(i) < 0.1) {
      toy_hist->SetBinContent(i, 0.0);
      toy_hist->SetBinError(i, 0.0);
      cout << " WARNING: setting value 0.0 to histo " << name << " for bin number " << i << endl;
    }
  }
  RooDataHist *toy_rooHist = new RooDataHist(name, name, RooArgList(*var), toy_hist);
  return toy_rooHist;
}

// a function to create the pdf used for the fit, need the histo model, should be zmm except for zmusta case.....

RooHistPdf *createHistPdf(const char *name, TH1F *model, RooRealVar *var, int rebin) {
  TH1F *model_clone = new TH1F(*model);
  model_clone->Sumw2();
  model_clone->Rebin(rebin);
  RooDataHist *model_dataHist = new RooDataHist(name, name, RooArgList(*var), model_clone);
  RooHistPdf *HistPdf = new RooHistPdf(name, name, *var, *model_dataHist, 0);
  delete model_clone;
  return HistPdf;
}

void fit(RooAbsReal &chi2, int numberOfBins, const char *outFileNameWithFitResult) {
  TFile *out_root_file = new TFile(outFileNameWithFitResult, "recreate");
  RooMinuit m_tot(chi2);
  m_tot.migrad();
  // m_tot.hesse();
  RooFitResult *r_chi2 = m_tot.save();
  cout << "==> Chi2 Fit results " << endl;
  r_chi2->Print("v");
  //  r_chi2->floatParsFinal().Print("v") ;
  int NumberOfFreeParameters = r_chi2->floatParsFinal().getSize();
  for (int i = 0; i < NumberOfFreeParameters; ++i) {
    r_chi2->floatParsFinal()[i].Print();
  }
  cout << "chi2:" << chi2.getVal() << ", numberOfBins: " << numberOfBins
       << ", NumberOfFreeParameters: " << NumberOfFreeParameters << endl;
  cout << "Normalized Chi2   = " << chi2.getVal() / (numberOfBins - NumberOfFreeParameters) << endl;
  r_chi2->Write();
  delete out_root_file;
}

int main(int argc, char **argv) {
  gROOT->SetStyle("Plain");
  double fMin, fMax, lumi, scaleLumi = 1;
  int seed;
  Bool_t toy = kFALSE;
  Bool_t fitFromData = kFALSE;
  string infile, outfile;
  int rebinZMuMu = 1, rebinZMuSa = 1, rebinZMuTk = 1, rebinZMuMuNoIso = 1, rebinZMuMuHlt = 1;
  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "produce help message")("toy,t", "toy enabled")(
      "seed,s", po::value<int>(&seed)->default_value(34567), "seed value for toy")(
      "luminosity,l", po::value<double>(&lumi)->default_value(45.), "luminosity value for toy ")(
      "fit,f", "fit from data enabled")(
      "rebin,r", po::value<vector<int> >(), "rebin value: r_mutrk r mumuNotIso r_musa r _muhlt")(
      "input-file,i", po::value<string>(&infile), "input file")(
      "output-file,o", po::value<string>(&outfile), "output file with fit results")(
      "min,m", po::value<double>(&fMin)->default_value(60), "minimum value for fit range")(
      "max,M", po::value<double>(&fMax)->default_value(120), "maximum value for fit range");
  po::positional_options_description p;
  p.add("rebin", -1);
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).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("toy")) {
    cout << "toy enabled with seed " << seed << "\n";
    toy = kTRUE;
    //RooRandom::randomGenerator()->SetSeed(seed) ;
    // lumi should be intented as pb-1 and passed from outside
    scaleLumi = lumi / 45.0;  // 45 is the current lumi correspondent to the histogram.....
  }
  if (vm.count("fit")) {
    cout << "fit from data enabled \n";
    fitFromData = kTRUE;
  }

  if (!vm.count("toy") && !vm.count("fit")) {
    cerr << "Choose one beetween  fitting form data or with a toy MC \n";
    return 1;
  }

  if (toy == fitFromData) {
    cerr << "Choose if fit from data or with a toy MC \n";
    return 1;
  }

  if (vm.count("rebin")) {
    vector<int> v_rebin = vm["rebin"].as<vector<int> >();
    if (v_rebin.size() != 4) {
      cerr << " please provide 4 numbers in the given order:r_mutrk r mumuNotIso r_musa r _muhlt \n";
      return 1;
    }
    rebinZMuTk = v_rebin[0];
    rebinZMuMuNoIso = v_rebin[1];
    rebinZMuSa = v_rebin[2];
    rebinZMuMuHlt = v_rebin[3];
  }

  RooRealVar *mass = new RooRealVar("mass", "mass (GeV/c^{2})", fMin, fMax);
  TFile *root_file = new TFile(infile.c_str(), "read");
  int range = (int)(fMax - fMin);
  int numberOfBins = range / rebinZMuSa + range / rebinZMuTk + range / rebinZMuMuNoIso + 2 * range / rebinZMuMuHlt;
  // zmm histograms used for pdf
  TH1F *zmm = getHisto(root_file, "goodZToMuMuPlots/zMass", fMin, fMax, rebinZMuMu);
  // zmsta used for pdf
  TH1F *zmsta = getHisto(root_file, "zmumuSaMassHistogram/zMass", fMin, fMax, 1);  // histogramms to fit.....
  TH1F *zmmsta = getHisto(root_file, "goodZToMuMuOneStandAloneMuonPlots/zMass", fMin, fMax, rebinZMuSa / rebinZMuMu);
  TH1F *zmt = getHisto(root_file, "goodZToMuMuOneTrackPlots/zMass", fMin, fMax, rebinZMuTk / rebinZMuMu);
  TH1F *zmmNotIso = getHisto(root_file, "nonIsolatedZToMuMuPlots/zMass", fMin, fMax, rebinZMuMuNoIso / rebinZMuMu);
  TH1F *zmm1hlt = getHisto(root_file, "goodZToMuMu1HLTPlots/zMass", fMin, fMax, rebinZMuMuHlt / rebinZMuMu);
  TH1F *zmm2hlt = getHisto(root_file, "goodZToMuMu2HLTPlots/zMass", fMin, fMax, rebinZMuMuHlt / rebinZMuMu);

  // creating a pdf for Zmt

  RooHistPdf *ZmtPdf = createHistPdf("ZmtPdf", zmm, mass, rebinZMuTk / rebinZMuMu);
  // creating a pdf for Zmm not iso
  RooHistPdf *ZmmNoIsoPdf = createHistPdf("ZmmNoIsoPdf", zmm, mass, rebinZMuMuNoIso / rebinZMuMu);
  // creating a pdf for Zms from zmsta!!!
  RooHistPdf *ZmsPdf = createHistPdf("ZmsPdf", zmsta, mass, rebinZMuSa / rebinZMuMu);
  // creating a pdf for Zmmhlt
  RooHistPdf *ZmmHltPdf = createHistPdf("ZmmHltPdf", zmm, mass, rebinZMuMuHlt / rebinZMuMu);

  // creating the variable with random init values

  RooRealVar Yield("Yield", "Yield", 15000., 345., 3567890.);
  RooRealVar nbkg_mutrk("nbkg_mutrk", "background _mutrk fraction", 500, 0., 100000.);
  RooRealVar nbkg_mumuNotIso("nbkg_mumuNotIso", "background fraction", 500, 0., 100000.);
  RooRealVar nbkg_musa("nbkg_musa", "background fraction", 50, 0., 100000.);
  RooRealVar eff_tk("eff_tk", "signal _mutrk fraction", .99, 0.8, 1.0);
  RooRealVar eff_sa("eff_sa", "eff musta", 0.99, 0.8, 1.0);
  RooRealVar eff_iso("eff_iso", "eff mumuNotIso", .99, 0.8, 1.0);
  RooRealVar eff_hlt("eff_hlt", "eff 1hlt", 0.99, 0.8, 1.0);
  RooRealVar alpha("alpha", "coefficient alpha", -0.01, -1000, 1000.);
  RooRealVar a0("a0", "coefficient 0", 1, -1000., 1000.);
  RooRealVar a1("a1", "coefficient 1", -0.001, -1000, 1000.);
  RooRealVar a2("a2", "coefficient 2", 0.0, -1000., 1000.);
  RooRealVar beta("beta", "coefficient beta", -0.01, -1000, 1000.);
  RooRealVar b0("b0", "coefficient 0", 1, -1000., 1000.);
  RooRealVar b1("b1", "coefficient 1", -0.001, -1000, 1000.);
  RooRealVar b2("b2", "coefficient 2", 0.0, -1000., 1000.);
  RooRealVar gamma("gamma", "coefficient gamma", -0.01, -1000, 1000.);
  RooRealVar c0("c0", "coefficient 0", 1, -1000., 1000.);
  RooRealVar c1("c1", "coefficient 1", -0.001, -1000, 1000.);
  // fit parameters setted from datacard
  filebuf fb;
  fb.open("zMuMuRooFit.txt", ios::in);
  istream is(&fb);
  char line[1000];

  Yield.readFromStream(is.getline(line, 1000), kFALSE);
  nbkg_mutrk.readFromStream(is.getline(line, 1000), kFALSE);
  nbkg_mumuNotIso.readFromStream(is.getline(line, 1000), kFALSE);
  nbkg_musa.readFromStream(is.getline(line, 1000), kFALSE);
  eff_tk.readFromStream(is.getline(line, 1000), kFALSE);
  eff_sa.readFromStream(is.getline(line, 1000), kFALSE);
  eff_iso.readFromStream(is.getline(line, 1000), kFALSE);
  eff_hlt.readFromStream(is.getline(line, 1000), kFALSE);
  alpha.readFromStream(is.getline(line, 1000), kFALSE);
  a0.readFromStream(is.getline(line, 1000), kFALSE);
  a1.readFromStream(is.getline(line, 1000), kFALSE);
  a2.readFromStream(is.getline(line, 1000), kFALSE);
  beta.readFromStream(is.getline(line, 1000), kFALSE);
  b0.readFromStream(is.getline(line, 1000), kFALSE);
  b1.readFromStream(is.getline(line, 1000), kFALSE);
  b2.readFromStream(is.getline(line, 1000), kFALSE);
  gamma.readFromStream(is.getline(line, 1000), kFALSE);
  c0.readFromStream(is.getline(line, 1000), kFALSE);
  c1.readFromStream(is.getline(line, 1000), kFALSE);
  fb.close();

  // scaling to lumi if toy is enabled...
  if (vm.count("toy")) {
    Yield.setVal(scaleLumi * (Yield.getVal()));
    nbkg_mutrk.setVal(scaleLumi * (nbkg_mutrk.getVal()));
    nbkg_mumuNotIso.setVal(scaleLumi * (nbkg_mumuNotIso.getVal()));
  }

  //efficiency term

  //zMuMuEff1HLTTerm = _2 * (effTk ^ _2) *  (effSa ^ _2) * (effIso ^ _2) * effHLT * (_1 - effHLT);
  RooFormulaVar zMuMu1HLTEffTerm(
      "zMuMu1HLTEffTerm",
      "Yield * (2.* (eff_tk)^2 * (eff_sa)^2 * (eff_iso)^2 * eff_hlt *(1.- eff_hlt))",
      RooArgList(eff_tk,
                 eff_sa,
                 eff_iso,
                 eff_hlt,
                 Yield));  //zMuMuEff2HLTTerm = (effTk ^ _2) *  (effSa ^ _2) * (effIso ^ _2) * (effHLT ^ _2) ;
  RooFormulaVar zMuMu2HLTEffTerm("zMuMu2HLTEffTerm",
                                 "Yield * ((eff_tk)^2 * (eff_sa)^2 * (eff_iso)^2 * (eff_hlt)^2)",
                                 RooArgList(eff_tk, eff_sa, eff_iso, eff_hlt, Yield));
  //zMuMuNoIsoEffTerm = (effTk ^ _2) * (effSa ^ _2) * (_1 - (effIso ^ _2)) * (_1 - ((_1 - effHLT)^_2));
  RooFormulaVar zMuMuNoIsoEffTerm("zMuMuNoIsoEffTerm",
                                  "Yield * ((eff_tk)^2 * (eff_sa)^2 * (1.- (eff_iso)^2) * (1.- ((1.-eff_hlt)^2)))",
                                  RooArgList(eff_tk, eff_sa, eff_iso, eff_hlt, Yield));
  //zMuTkEffTerm = _2 * (effTk ^ _2) * effSa * (_1 - effSa) * (effIso ^ _2) * effHLT;
  RooFormulaVar zMuTkEffTerm("zMuTkEffTerm",
                             "Yield * (2. *(eff_tk)^2 * eff_sa * (1.-eff_sa)* (eff_iso)^2 * eff_hlt)",
                             RooArgList(eff_tk, eff_sa, eff_iso, eff_hlt, Yield));
  //zMuSaEffTerm = _2 * (effSa ^ _2) * effTk * (_1 - effTk) * (effIso ^ _2) * effHLT;
  RooFormulaVar zMuSaEffTerm("zMuSaEffTerm",
                             "Yield * (2. *(eff_sa)^2 * eff_tk * (1.-eff_tk)* (eff_iso)^2 * eff_hlt)",
                             RooArgList(eff_tk, eff_sa, eff_iso, eff_hlt, Yield));

  // creating model for the  fit
  // z mu track

  RooGenericPdf *bkg_mutrk = new RooGenericPdf("bkg_mutrk",
                                               "zmt bkg_model",
                                               "exp(alpha*mass) * ( a0 + a1 * mass + a2 * mass^2 )",
                                               RooArgSet(*mass, alpha, a0, a1, a2));
  // RooFormulaVar fracSigMutrk("fracSigMutrk", "@0 / (@0 + @1)", RooArgList(zMuTkEffTerm, nbkg_mutrk ));
  RooAddPdf *model_mutrk = new RooAddPdf(
      "model_mutrk", "model_mutrk", RooArgList(*ZmtPdf, *bkg_mutrk), RooArgList(zMuTkEffTerm, nbkg_mutrk));
  // z mu mu not Iso

  // creating background pdf for zmu mu not Iso
  RooGenericPdf *bkg_mumuNotIso = new RooGenericPdf("bkg_mumuNotIso",
                                                    "zmumuNotIso bkg_model",
                                                    "exp(beta * mass) * (b0 + b1 * mass + b2 * mass^2)",
                                                    RooArgSet(*mass, beta, b0, b1, b2));
  // RooFormulaVar fracSigMuMuNoIso("fracSigMuMuNoIso", "@0 / (@0 + @1)", RooArgList(zMuMuNoIsoEffTerm, nbkg_mumuNotIso ));
  RooAddPdf *model_mumuNotIso = new RooAddPdf("model_mumuNotIso",
                                              "model_mumuNotIso",
                                              RooArgList(*ZmmNoIsoPdf, *bkg_mumuNotIso),
                                              RooArgList(zMuMuNoIsoEffTerm, nbkg_mumuNotIso));
  // z mu sta

  // RooGenericPdf model_musta("model_musta",  " ZmsPdf * zMuSaEffTerm ", RooArgSet( *ZmsPdf, zMuSaEffTerm)) ;
  RooGenericPdf *bkg_musa = new RooGenericPdf(
      "bkg_musa", "zmusa bkg_model", "exp(gamma * mass) * (c0 + c1 * mass )", RooArgSet(*mass, gamma, c0, c1));
  // RooAddPdf * eZmsSig= new RooAddPdf("eZmsSig","eZmsSig",RooArgList(*,*bkg_mumuNotIso), RooArgList(zMuMuNoIsoEffTerm, nbkg_mumuNotIso));
  RooAddPdf *eZmsSig =
      new RooAddPdf("eZmsSig", "eZmsSig", RooArgList(*ZmsPdf, *bkg_musa), RooArgList(zMuSaEffTerm, nbkg_musa));

  //RooExtendPdf * eZmsSig= new RooExtendPdf("eZmsSig","extended signal p.d.f for zms ",*ZmsPdf,  zMuSaEffTerm ) ;

  // z mu mu HLT

  // count ZMuMu Yield
  double nZMuMu = 0.;
  double nZMuMu1HLT = 0.;
  double nZMuMu2HLT = 0.;
  unsigned int nBins = zmm->GetNbinsX();
  double xMin = zmm->GetXaxis()->GetXmin();
  double xMax = zmm->GetXaxis()->GetXmax();
  double deltaX = (xMax - xMin) / nBins;
  for (unsigned int i = 0; i < nBins; ++i) {
    double x = xMin + (i + .5) * deltaX;
    if (x > fMin && x < fMax) {
      nZMuMu += zmm->GetBinContent(i + 1);
      nZMuMu1HLT += zmm1hlt->GetBinContent(i + 1);
      nZMuMu2HLT += zmm2hlt->GetBinContent(i + 1);
    }
  }

  cout << ">>> count of ZMuMu yield in the range [" << fMin << ", " << fMax << "]: " << nZMuMu << endl;
  cout << ">>> count of ZMuMu (1HLT) yield in the range [" << fMin << ", " << fMax << "]: " << nZMuMu1HLT << endl;
  cout << ">>> count of ZMuMu (2HLT) yield in the range [" << fMin << ", " << fMax << "]: " << nZMuMu2HLT << endl;
  // we set eff_hlt
  //eff_hlt.setVal( 1. / (1. + (nZMuMu1HLT/ (2 * nZMuMu2HLT))) ) ;
  // creating the pdf for z mu mu 1hlt

  RooExtendPdf *eZmm1hltSig =
      new RooExtendPdf("eZmm1hltSig", "extended signal p.d.f for zmm 1hlt", *ZmmHltPdf, zMuMu1HLTEffTerm);
  // creating the pdf for z mu mu 2hlt
  RooExtendPdf *eZmm2hltSig =
      new RooExtendPdf("eZmm2hltSig", "extended signal p.d.f for zmm 2hlt", *ZmmHltPdf, zMuMu2HLTEffTerm);

  // getting the data if fit otherwise constructed the data for model if toy....

  RooDataHist *zmtMass, *zmmNotIsoMass, *zmsMass, *zmm1hltMass, *zmm2hltMass;

  if (toy) {
    zmtMass = genHistFromModelPdf("zmtMass", model_mutrk, mass, scaleLumi, range, rebinZMuTk, seed);
    zmmNotIsoMass =
        genHistFromModelPdf("zmmNotIsoMass", model_mumuNotIso, mass, scaleLumi, range, rebinZMuMuNoIso, seed);
    zmsMass = genHistFromModelPdf("zmsMass", eZmsSig, mass, scaleLumi, range, rebinZMuSa, seed);
    zmm1hltMass = genHistFromModelPdf("zmm1hltMass", eZmm1hltSig, mass, scaleLumi, range, rebinZMuMuHlt, seed);
    zmm2hltMass = genHistFromModelPdf("zmm2hltMass", eZmm2hltSig, mass, scaleLumi, range, rebinZMuMuHlt, seed);
  } else {  // if  fit from data....
    zmtMass = new RooDataHist("zmtMass", "good z mu track", RooArgList(*mass), zmt);
    zmmNotIsoMass = new RooDataHist("ZmmNotIso", "good z mu mu not isolated", RooArgList(*mass), zmmNotIso);
    zmsMass = new RooDataHist("zmsMass", "good z mu sta mass", RooArgList(*mass), zmmsta);
    zmm1hltMass = new RooDataHist("zmm1hltMass", "good Z mu mu 1hlt", RooArgList(*mass), zmm1hlt);
    zmm2hltMass = new RooDataHist("zmm2hltMass", "good Z mu mu 2hlt", RooArgList(*mass), zmm2hlt);
  }

  // creting the chi2s
  RooChi2Var *chi2_mutrk =
      new RooChi2Var("chi2_mutrk", "chi2_mutrk", *model_mutrk, *zmtMass, Extended(kTRUE), DataError(RooAbsData::SumW2));
  RooChi2Var *chi2_mumuNotIso = new RooChi2Var("chi2_mumuNotIso",
                                               "chi2_mumuNotIso",
                                               *model_mumuNotIso,
                                               *zmmNotIsoMass,
                                               Extended(kTRUE),
                                               DataError(RooAbsData::SumW2));

  RooChi2Var *chi2_musta =
      new RooChi2Var("chi2_musta", "chi2_musta", *eZmsSig, *zmsMass, Extended(kTRUE), DataError(RooAbsData::SumW2));
  // uncomment this line if you want to use logLik for mu sta
  // RooNLLVar *chi2_musta = new RooNLLVar("chi2_musta","chi2_musta",*eZmsSig, *zmsMass,  Extended(kTRUE), DataError(RooAbsData::SumW2) ) ;
  RooChi2Var *chi2_mu1hlt = new RooChi2Var(
      "chi2_mu1hlt", "chi2_mu1hlt", *eZmm1hltSig, *zmm1hltMass, Extended(kTRUE), DataError(RooAbsData::SumW2));
  RooChi2Var *chi2_mu2hlt = new RooChi2Var(
      "chi2_mu2hlt", "chi2_mu2hlt", *eZmm2hltSig, *zmm2hltMass, Extended(kTRUE), DataError(RooAbsData::SumW2));

  // adding the chi2
  RooAddition totChi2("totChi2",
                      "chi2_mutrk + chi2_mumuNotIso  + chi2_musta   + chi2_mu1hlt  +  chi2_mu2hlt ",
                      RooArgSet(*chi2_mutrk, *chi2_mumuNotIso, *chi2_musta, *chi2_mu1hlt, *chi2_mu2hlt));

  // printing out the model integral befor fitting
  double N_zMuMu1hlt = eZmm1hltSig->expectedEvents(*mass);
  double N_bkgTk = bkg_mutrk->expectedEvents(*mass);
  double N_bkgIso = bkg_mumuNotIso->expectedEvents(*mass);

  double N_zMuTk = zMuTkEffTerm.getVal();

  double e_hlt = eff_hlt.getVal();
  double e_tk = eff_tk.getVal();
  double e_sa = eff_sa.getVal();
  double e_iso = eff_iso.getVal();
  double Y_hlt = N_zMuMu1hlt / ((2. * (e_tk * e_tk) * (e_sa * e_sa) * (e_iso * e_iso) * e_hlt * (1. - e_hlt)));
  double Y_mutk = N_zMuTk / ((2. * (e_tk * e_tk) * e_sa * (1. - e_sa) * (e_iso * e_iso) * e_hlt));

  cout << "Yield prediction from mumu1hlt integral befor fitting: " << Y_hlt << endl;
  cout << "Yield prediction from mutk integral befor fitting: " << Y_mutk << endl;
  cout << "Bkg for mutk prediction from mutk integral after fitting: " << N_bkgTk << endl;
  cout << "Bkg for mumuNotIso prediction from mumuNotIso integral after fitting: " << N_bkgIso << endl;

  fit(totChi2, numberOfBins, outfile.c_str());
  N_zMuMu1hlt = eZmm1hltSig->expectedEvents(*mass);
  N_zMuTk = zMuTkEffTerm.getVal();
  double N_zMuMuNoIso = zMuMuNoIsoEffTerm.getVal();
  double N_Tk = model_mutrk->expectedEvents(*mass);
  double N_Iso = model_mumuNotIso->expectedEvents(*mass);
  e_hlt = eff_hlt.getVal();
  e_tk = eff_tk.getVal();
  e_sa = eff_sa.getVal();
  e_iso = eff_iso.getVal();
  Y_hlt = N_zMuMu1hlt / ((2. * (e_tk * e_tk) * (e_sa * e_sa) * (e_iso * e_iso) * e_hlt * (1. - e_hlt)));
  Y_mutk = N_zMuTk / ((2. * (e_tk * e_tk) * e_sa * (1. - e_sa) * (e_iso * e_iso) * e_hlt));

  cout << "Yield prediction from mumu1hlt integral after fitting: " << Y_hlt << endl;
  //cout << "Yield prediction from mutk integral after fitting: " <<  Y_mutk << endl;
  cout << "N + B  prediction from mutk integral after fitting: " << N_Tk << endl;
  cout << "zMuTkEffTerm " << N_zMuTk << endl;
  cout << "N + B  prediction from mumuNotIso integral after fitting: " << N_Iso << endl;
  cout << "zMuMuNoIsoEffTerm " << N_zMuMuNoIso << endl;

  cout << "chi2_mutrk:" << chi2_mutrk->getVal() << endl;
  cout << "chi2_mumuNotIso:" << chi2_mumuNotIso->getVal() << endl;
  cout << "chi2_musta:" << chi2_musta->getVal() << endl;
  cout << "chi2_mumu1hlt:" << chi2_mu1hlt->getVal() << endl;
  cout << "chi2_mumu2hlt:" << chi2_mu2hlt->getVal() << endl;

  //plotting
  RooPlot *massFrame_mutrk = mass->frame();
  RooPlot *massFrame_mumuNotIso = mass->frame();
  RooPlot *massFrame_musta = mass->frame();
  RooPlot *massFrame_mumu1hlt = mass->frame();
  RooPlot *massFrame_mumu2hlt = mass->frame();

  TCanvas *canv1 = new TCanvas("canvas");
  TCanvas *canv2 = new TCanvas("new_canvas");
  canv1->Divide(2, 3);
  canv1->cd(1);
  canv1->SetLogy(kTRUE);
  zmtMass->plotOn(massFrame_mutrk, LineColor(kBlue));
  model_mutrk->plotOn(massFrame_mutrk, LineColor(kRed));
  model_mutrk->plotOn(massFrame_mutrk, Components(*bkg_mutrk), LineColor(kGreen));
  massFrame_mutrk->SetTitle("Z -> #mu track");
  // massFrame_mutrk->GetYaxis()->SetLogScale();
  massFrame_mutrk->Draw();
  gPad->SetLogy(1);

  canv2->cd();
  canv2->SetLogy(kTRUE);
  massFrame_mutrk->Draw();
  canv2->SaveAs("LogZMuTk.eps");
  canv2->SetLogy(kFALSE);
  canv2->SaveAs("LinZMuTk.eps");

  canv1->cd(2);
  zmmNotIsoMass->plotOn(massFrame_mumuNotIso, LineColor(kBlue));
  model_mumuNotIso->plotOn(massFrame_mumuNotIso, LineColor(kRed));
  model_mumuNotIso->plotOn(massFrame_mumuNotIso, Components(*bkg_mumuNotIso), LineColor(kGreen));
  massFrame_mumuNotIso->SetTitle("Z -> #mu #mu not isolated");
  massFrame_mumuNotIso->Draw();
  gPad->SetLogy(1);

  canv2->cd();
  canv2->Clear();
  canv2->SetLogy(kTRUE);
  massFrame_mumuNotIso->Draw();
  canv2->SaveAs("LogZMuMuNotIso.eps");
  canv2->SetLogy(kFALSE);
  canv2->SaveAs("LinZMuMuNotIso.eps");

  canv1->cd(3);
  zmsMass->plotOn(massFrame_musta, LineColor(kBlue));
  eZmsSig->plotOn(massFrame_musta, Components(*bkg_musa), LineColor(kGreen));
  eZmsSig->plotOn(massFrame_musta, LineColor(kRed));
  massFrame_musta->SetTitle("Z -> #mu sta");
  massFrame_musta->Draw();

  canv2->cd();
  canv2->Clear();
  canv2->SetLogy(kTRUE);
  massFrame_musta->Draw();
  canv2->SaveAs("LogZMuSa.eps");
  canv2->SetLogy(kFALSE);
  canv2->SaveAs("LinZMuSa.eps");

  canv1->cd(4);
  zmm1hltMass->plotOn(massFrame_mumu1hlt, LineColor(kBlue));
  eZmm1hltSig->plotOn(massFrame_mumu1hlt, LineColor(kRed));
  massFrame_mumu1hlt->SetTitle("Z -> #mu #mu 1hlt");
  massFrame_mumu1hlt->Draw();
  canv2->cd();
  canv2->Clear();
  canv2->SetLogy(kTRUE);
  massFrame_mumu1hlt->Draw();
  canv2->SaveAs("LogZMuMu1Hlt.eps");
  canv2->SetLogy(kFALSE);
  canv2->SaveAs("LinZMuMu1Hlt.eps");

  canv1->cd(5);
  zmm2hltMass->plotOn(massFrame_mumu2hlt, LineColor(kBlue));
  eZmm2hltSig->plotOn(massFrame_mumu2hlt, LineColor(kRed));
  massFrame_mumu2hlt->SetTitle("Z -> #mu #mu 2hlt");
  massFrame_mumu2hlt->Draw();

  canv1->SaveAs("mass.eps");

  canv2->cd();
  canv2->Clear();
  canv2->SetLogy(kTRUE);
  massFrame_mumu2hlt->Draw();
  canv2->SaveAs("LogZMuMu2Hlt.eps");
  canv2->SetLogy(kFALSE);
  canv2->SaveAs("LinZMuMu2Hlt.eps");

  /* how to read the fit result in root
     TH1D h_Yield("h_Yield", "h_Yield", 100, 10000, 30000)
     for (int i =0: i < 100; i++){
     RooFitResult* r = gDirectory->Get(Form("toy_totChi2;%d)",i)
     //r->floatParsFinal().Print("s");
     // without s return a list,  can we get the number?
     RooFitResult* r = gDirectory->Get("toy_totChi2;1")
     // chi2
     r->minNll();
     //distamce form chi2.....
     //r->edm();
     // yield
     r->floatParsFinal()[0]->Print();
     //RooAbsReal * l = r->floatParsFinal()->first()
     RooAbsReal * y = r->floatParsFinal()->find("Yield");
     h_Yield->Fill(y->getVal());
     }

  */

  delete root_file;
  //delete out_root_file;
  return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/EarthOrientation/polarMotionCalculator.h"
#include "Tudat/Astrodynamics/EarthOrientation/earthOrientationCalculator.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::earth_orientation;

BOOST_AUTO_TEST_SUITE( test_polar_motion_calculator )

//! Check correct combination of data from the two contributions of polar motion: short-period terms and IERS daily values.
BOOST_AUTO_TEST_CASE( testPolarMotionCalculator )
{
    // Retrieve polar motion calculator
    std::shared_ptr< EarthOrientationAnglesCalculator > standardEarthRotationModel =
            createStandardEarthOrientationCalculator( );
    std::shared_ptr< PolarMotionCalculator > standardPolarMotionCalculator =
            standardEarthRotationModel->getPolarMotionCalculator( );

    // Get constituent polar motion calculation objects.
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Vector2d > >
        dailyPolarMotionValueInterpolator = standardPolarMotionCalculator->getDailyIersValueInterpolator( );
    std::shared_ptr< ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > > shortPeriodPolarMotionCalculator =
            getDefaultPolarMotionCorrectionCalculator( );

    // Define test time
    double testEphemerisTime = 1.0E8;
    double testUtc = sofa_interface::convertTTtoUTC( testEphemerisTime );

    // Compute fundamental arguments
    Eigen::Vector6d fundamentalArguments = sofa_interface::calculateApproximateDelaunayFundamentalArgumentsWithGmst( testEphemerisTime );

    // Compute polar motion from both interfaces (time and arguments)
    Eigen::Vector2d totalPolarMotionFromTime =
            standardPolarMotionCalculator->getPositionOfCipInItrs( testEphemerisTime, testUtc );
    Eigen::Vector2d totalPolarMotionFromArguments =
            standardPolarMotionCalculator->getPositionOfCipInItrs( fundamentalArguments, testUtc );

    // Check combinations fo calculations
    BOOST_CHECK_EQUAL( totalPolarMotionFromTime.x( ), ( dailyPolarMotionValueInterpolator->interpolate( testUtc ) +
                       shortPeriodPolarMotionCalculator->getCorrections( testEphemerisTime ) ).x( ) );
    BOOST_CHECK_EQUAL( totalPolarMotionFromTime.y( ), ( dailyPolarMotionValueInterpolator->interpolate( testUtc ) +
                       shortPeriodPolarMotionCalculator->getCorrections( testEphemerisTime ) ).y( ) );

    BOOST_CHECK_EQUAL( totalPolarMotionFromArguments.x( ), ( dailyPolarMotionValueInterpolator->interpolate( testUtc ) +
                       shortPeriodPolarMotionCalculator->getCorrections( testEphemerisTime ) ).x( ) );
    BOOST_CHECK_EQUAL( totalPolarMotionFromArguments.y( ), ( dailyPolarMotionValueInterpolator->interpolate( testUtc ) +
                       shortPeriodPolarMotionCalculator->getCorrections( testEphemerisTime ) ).y( ) );

    BOOST_CHECK_EQUAL( totalPolarMotionFromArguments.x( ), ( dailyPolarMotionValueInterpolator->interpolate( testUtc ) +
                       shortPeriodPolarMotionCalculator->getCorrections( fundamentalArguments ) ).x( ) );
    BOOST_CHECK_EQUAL( totalPolarMotionFromArguments.y( ), ( dailyPolarMotionValueInterpolator->interpolate( testUtc ) +
                       shortPeriodPolarMotionCalculator->getCorrections( fundamentalArguments ) ).y( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  //------------------------------------------------------------------------------------------------
  //  float --> int32
  BOOST_DISPATCH_OVERLOAD ( pack_cast_
                          , (typename A0, typename A1)
                          , bs::sse2_
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                          , bd::target_<bd::scalar_<bd::int32_<A1>>>
                          )
  {
    using result = typename A0::template rebind<typename A1::type>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0, A1 const& ) const BOOST_NOEXCEPT
    {
      return _mm_cvttps_epi32(a0);
    }
  };

  //------------------------------------------------------------------------------------------------
  //  int32 --> float
  BOOST_DISPATCH_OVERLOAD ( pack_cast_
                          , (typename A0, typename A1)
                          , bs::sse2_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bd::target_<bd::scalar_<bd::single_<A1>>>
                          )
  {
    using result = typename A0::template rebind<typename A1::type>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0, A1 const& ) const BOOST_NOEXCEPT
    {
      return _mm_cvtepi32_ps(a0);
    }
  };
} } }

#endif


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Core/FundamentalTypes.h>
#include <vw/Math/Vector.h>
#include <asp/IsisIO/PolyEquation.h>

#include <iomanip>
#include <vector>
#include <string>
#include <algorithm>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

using namespace vw;
using namespace asp;

// Constructors
//---------------------------------------------
PolyEquation::PolyEquation ( int order ) {
  if ( order < 0 )
    vw_throw( ArgumentErr() << "PolyEquation: Polynomial order must be greater than zero." );
  if ( order > 254 )
    vw_throw( ArgumentErr() << "PolyEquation: Polynomial order must be less than 255" );
  m_x_coeff.set_size( order + 1 );
  m_y_coeff.set_size( order + 1 );
  m_z_coeff.set_size( order + 1 );
  for ( int i = 0; i < order+1; i++ )
    m_x_coeff[i] = m_y_coeff[i] = m_z_coeff[i] = 0;
  m_cached_time = -1;
  m_time_offset = 0;
  m_max_length = uint8(order)+1;
}
PolyEquation::PolyEquation( int order_x,
                            int order_y,
                            int order_z ) {
  if ( order_x < 0 || order_y < 0 || order_z < 0 )
    vw_throw( ArgumentErr() << "PolyEquation: Polynomial order must be greater than zero." );
  if ( order_x > 254 || order_y > 254 || order_z > 254 )
    vw_throw( ArgumentErr() << "PolyEquation: Polynomial order must be less than 255" );
  m_x_coeff.set_size(order_x+1);
  m_y_coeff.set_size(order_y+1);
  m_z_coeff.set_size(order_z+1);
  for ( unsigned i = 0; i < m_x_coeff.size(); i++ )
    m_x_coeff[i] = 0;
  for ( unsigned i = 0; i < m_y_coeff.size(); i++ )
    m_y_coeff[i] = 0;
  for ( unsigned i = 0; i < m_z_coeff.size(); i++ )
    m_z_coeff[i] = 0;
  m_cached_time = -1;
  m_time_offset = 0;
  m_max_length = uint8( std::max( order_x, std::max( order_y, order_z ) ) ) + 1;
}

// Update
//-----------------------------------------------
void PolyEquation::update( double t ) {
  m_cached_time = t;
  double delta_t = t-m_time_offset;
  Vector<double> powers( m_max_length );
  powers[0] = 1;
  for ( uint8 i = 1; i < m_max_length; i++ )
    powers[i] = powers[i-1]*delta_t;
  m_cached_output[0] = sum( elem_prod(m_x_coeff,
                                      subvector(powers,0,m_x_coeff.size())) );
  m_cached_output[1] = sum( elem_prod(m_y_coeff,
                                      subvector(powers,0,m_y_coeff.size())) );
  m_cached_output[2] = sum( elem_prod(m_z_coeff,
                                      subvector(powers,0,m_z_coeff.size())) );
}

// FileIO
//-----------------------------------------------
void PolyEquation::write( std::ofstream& f ) {
  for ( int i = 0; i < 3; i++ ) {
    Vector<double>* pointer;
    switch(i) {
    case 0:
      pointer = &m_x_coeff;
      break;
    case 1:
      pointer = &m_y_coeff;
      break;
    default:
    case 2:
      pointer = &m_z_coeff;
      break;
    }

    f << std::setprecision( 15 );
    for ( unsigned j = 0; j < (*pointer).size(); j++ )
      f << (*pointer)[j] << " ";
    f << "\n";
  }
}

void PolyEquation::read( std::ifstream& f ) {
  std::string buffer;
  std::vector<std::string> tokens;
  m_cached_time = -1;
  for ( int i = 0; i < 3; i++ ) {
    buffer = "";
    std::getline( f, buffer );
    boost::split( tokens, buffer, boost::is_any_of(" =\n") );

    // Cleaning out any tokens that are just ""
    for(std::vector<std::string>::iterator iter = tokens.begin();
        iter != tokens.end(); ++iter )
      if ( (*iter) == "" ) {
        iter = tokens.erase(iter);
        iter--;
      }

    Vector<double>* pointer;
    switch(i) {
    case 0:
      pointer = &m_x_coeff;
      break;
    case 1:
      pointer = &m_y_coeff;
      break;
    default:
    case 2:
      pointer = &m_z_coeff;
      break;
    }

    pointer->set_size( tokens.size() );
    for ( unsigned j = 0; j < tokens.size(); j++ )
      (*pointer)[j] = atof( tokens[j].c_str() );

  }
}

// Constant Access
//-----------------------------------------------
double& PolyEquation::operator[]( size_t n ) {
  m_cached_time = -1;
  if (n >= m_x_coeff.size() + m_y_coeff.size() + m_z_coeff.size())
    vw_throw(ArgumentErr() << "PolyEquation: invalid index.");
  if (n < m_x_coeff.size()) {
    return m_x_coeff[n];
  } else if (n < m_x_coeff.size() + m_y_coeff.size()) {
    return m_y_coeff[n - m_x_coeff.size()];
  } else {
    return m_z_coeff[n - m_x_coeff.size() - m_y_coeff.size()];
  }
}


/*
  Operators

  Copyright 2010-201x held jointly by LANS/LANL, LBNL, and PNNL.
  Amanzi is released under the three-clause BSD License.
  The terms of use and "as is" disclaimer for this license are
  provided in the top-level COPYRIGHT file.

  Author: Daniil Svyatskiy (dasvyat@lanl.gov)
*/

#ifndef AMANZI_BOUNDARY_FLUX_FUNC
#define AMANZI_BOUNDARY_FLUX_FUNC

#include <boost/math/tools/roots.hpp>

namespace Amanzi {

#define DEBUG_FLAG 0

/* ******************************************************************
* Nonlinear function F(x) = k(p0 - x) * [(T*dir) * (p - x) + g] - bc
* where k is the nonlinear function provided by the model
* and p0, T, dir, p, g, bc are constant parameters.
****************************************************************** */
template <class Model>
class BoundaryFluxFn {
 public:

  typedef double(Model::*NonlinFunc)(double p) const;

  BoundaryFluxFn(const double trans_f,
                 const double lambda,
                 double cell_p,
                 double bc_flux,
                 double g_flux,
                 int dir,
                 double patm,
                 Teuchos::RCP<const Model> model,
                 NonlinFunc nonlinfunc):
      trans_f_(trans_f), lambda_(lambda),
      cell_p_(cell_p), bc_flux_(bc_flux), g_flux_(g_flux),
      dir_(dir), patm_(patm) {
    model_ = model;
    nonlinfunc_ = nonlinfunc;
  }

  double operator()(double face_p) {
    lambda_ = face_p;
    double krel = ((*model_).*nonlinfunc_)(patm_ - lambda_);
    double flux = dir_ * trans_f_ * krel * (cell_p_ - lambda_);

    return flux + g_flux_ * krel - bc_flux_;
  }

 protected:
  double trans_f_;
  double lambda_;
  int face_index_;
  double face_Mff_;
  double cell_p_;
  double bc_flux_;
  double g_flux_;
  int dir_;
  double patm_;
  Teuchos::RCP<const Model> model_;
  NonlinFunc nonlinfunc_;
};

/* ******************************************************************
* Auxiliaty class for toms748: convergence criteria.
****************************************************************** */
struct Tol_ {
  Tol_(double eps) : eps_(eps) {};
  bool operator()(const double& a, const double& b) const {
    return std::abs(a - b) <= eps_;
  }
  double eps_;
};

/* ******************************************************************
* Bisection solver based on toms748 algorithm.
****************************************************************** */
template <class Model>
class BoundaryFaceSolver {
 public:
  typedef double(Model::*NonlinFunc)(double p) const;

  BoundaryFaceSolver(double trans_f, double g_f,
                     double cell_val, double lambda,
                     double bnd_flux, int dir, double patm,
                     double min_val, double max_val, double eps,
                     Teuchos::RCP<const Model> model, NonlinFunc test_fun) :
    trans_f_(trans_f),
    g_f_(g_f),
    cell_val_(cell_val),
    lambda_(lambda),
    patm_(patm),
    min_val_(min_val),
    max_val_(max_val),
    eps_(eps),
    bnd_flux_(bnd_flux)
  {
    func_ = Teuchos::rcp(new BoundaryFluxFn<Model>(trans_f, lambda, cell_val, bnd_flux,
                                                   g_f, dir, patm, model, test_fun));
  }

  double SolveBisection(double face_val,
                        double min_val,
                        double max_val,
                        BoundaryFluxFn<Model>& func,
                        Tol_& tol,
                        boost::uintmax_t max_it,
                        boost::uintmax_t& actual_it);

  double FaceValue() {
    Tol_ tol(eps_);
    boost::uintmax_t max_it = 100;
    boost::uintmax_t actual_it(max_it);

    return SolveBisection(lambda_, min_val_, max_val_, *func_, tol, max_it, actual_it);
  }

  double lambda_;  // initial guess
  double cell_val_, trans_f_, g_f_, patm_, bnd_flux_;  // input data
  double min_val_, max_val_;  // bracket values
  double eps_;
  Teuchos::RCP<BoundaryFluxFn<Model> > func_;
};

template <class Model>
double BoundaryFaceSolver<Model>::SolveBisection(double face_val,
                                                 double min_val,
                                                 double max_val,
                                                 BoundaryFluxFn<Model>& func,
                                                 Tol_& tol,
                                                 boost::uintmax_t max_it,
                                                 boost::uintmax_t& actual_it)
{
  double res = func(face_val);
  double left(0.0), right(0.0), lres(0.0), rres(0.0);

#if DEBUG_FLAG
std::cout << "STaRT interval: (" << min_val << " " << max_val << "),  fval=" << face_val << "\n";
#endif
  if (res > 0.0) {
    left = face_val;
    lres = res;
    right = std::max(face_val, max_val);
    rres = func(right);
#if DEBUG_FLAG
std::cout << "---set right:      " << left << " (" << lres << "), " << right << " (" << rres << ")\n";
#endif
    while (rres > 0.0) {
      right += std::max(fabs(right), 100.0);
      rres = func(right);
#if DEBUG_FLAG
std::cout << "   change right:   " << right << " (" << rres << ")\n";
#endif
    }
  } else {
    right = face_val;
    rres = res;
    left = std::min(min_val, face_val);
    lres = func(left);
#if DEBUG_FLAG
std::cout << "---set left:       " << left << " (" << lres << "), " << right << " (" << rres << ")\n";
#endif
    while (lres < 0.0) {
      left -= std::max(fabs(left), 100.0);
      lres = func(left);
#if DEBUG_FLAG
std::cout << "   change left:    " << left << " (" << lres << ")\n";
#endif
    }
  }
#if DEBUG_FLAG
std::cout << "+++bracket (func): " << left << " (" << lres << "), " << right << " (" << rres << ")\n";
#endif

  if (tol(left, right)) {
    face_val = right;
    actual_it = 0;
  } else {
    std::pair<double, double> result;
    result = boost::math::tools::toms748_solve(func, left, right, lres, rres, tol, actual_it);
    // if (actual_it >= max_it) {
    //   std::cout << " Failed to converged in " << actual_it << " steps." << std::endl;
    // }
    face_val = (result.first + result.second) / 2.0;
  }

#if DEBUG_FLAG
std::cout << "   solution = " << face_val << " itrs = " << actual_it << "\n";
#endif
  return face_val;
}

}  // namespace Amanzi

#endif


// Copyright 2019 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#include <ndt/ndt_voxel.hpp>
#include <ndt/utils.hpp>
#include <Eigen/LU>
#include <limits>
#include "common/types.hpp"

using autoware::common::types::bool8_t;

namespace autoware
{
namespace localization
{
namespace ndt
{
constexpr uint32_t DynamicNDTVoxel::NUM_POINT_THRESHOLD;

DynamicNDTVoxel::DynamicNDTVoxel()
{
  // default constructors use uninitialized values.
  m_centroid.setZero();
  m_M2.setZero();
  m_covariance.setZero();
}

void DynamicNDTVoxel::add_observation(const Point & pt)
{
  const auto last_count = m_num_points++;

  const auto last_centroid = m_centroid;
  const auto last_delta = pt - last_centroid;
  m_centroid = last_centroid + (last_delta / m_num_points);

  const auto current_delta = pt - m_centroid;
  m_M2 += last_delta * current_delta.transpose();

  if (usable()) {
    // TODO(yunus.caliskan): Apply numerical stability enhancing steps described in:
    // http://www.diva-portal.org/smash/get/diva2:276162/FULLTEXT02.pdf, pg 60
    m_covariance = m_M2 / last_count;
  }

  // set invertibility to unknown since the covariance has changed
  m_invertible = Invertibility::UNKNOWN;
}

bool8_t DynamicNDTVoxel::try_stabilize()
{
  bool8_t invertible = try_stabilize_covariance(m_covariance);
  if (invertible) {
    m_invertible = Invertibility::INVERTIBLE;
  } else {
    m_invertible = Invertibility::NOT_INVERTIBLE;
  }

  return invertible;
}

bool8_t DynamicNDTVoxel::usable() const noexcept
{
  return m_num_points >= NUM_POINT_THRESHOLD;
}

const Eigen::Matrix3d & DynamicNDTVoxel::covariance() const
{
  if (!usable()) {
    throw std::out_of_range(
            "DynamicNDTVoxel: Cannot get covariance from a "
            "voxel without sufficient number of points");
  }
  return m_covariance;
}

std::experimental::optional<Eigen::Matrix3d> DynamicNDTVoxel::inverse_covariance() const
{
  if (!usable()) {
    throw std::out_of_range(
            "DynamicNDTVoxel: Cannot get covariance from a "
            "voxel without sufficient number of points");
  }

  if (m_invertible == Invertibility::NOT_INVERTIBLE) {
    // if stabilization has been performed and covariance is not invertible
    return {};
  }

  Eigen::Matrix3d inv_covariance;
  bool8_t invertible;
  m_covariance.computeInverseWithCheck(inv_covariance, invertible);
  if (invertible) {
    return inv_covariance;
  } else {
    return {};
  }
}

const Eigen::Vector3d & DynamicNDTVoxel::centroid() const
{
  // Using the overloaded function as the parent function will use the hidden occupancy check
  if (!usable()) {
    throw std::out_of_range("DynamicNDTVoxel: Cannot get centroid from an unoccupied voxel");
  }
  return m_centroid;
}

uint64_t DynamicNDTVoxel::count() const noexcept
{
  return m_num_points;
}

/////////////////////////////////////////////////

StaticNDTVoxel::StaticNDTVoxel()
{
  m_centroid.setZero();
  m_inv_covariance.setZero();
}

StaticNDTVoxel::StaticNDTVoxel(const Point & centroid, const Cov & inv_covariance)
: m_centroid{centroid}, m_inv_covariance{inv_covariance}, m_occupied{true}
{}

Eigen::Matrix3d StaticNDTVoxel::covariance() const
{
  Eigen::Matrix3d covariance;
  if (m_occupied) {
    bool8_t invertible{false};
    m_inv_covariance.computeInverseWithCheck(covariance, invertible);
    if (!invertible) {
      throw std::out_of_range("StaticNDTVoxel: Inverse covariance is not invertible");
    }
  } else {
    throw std::out_of_range("StaticNDTVoxel: Cannot get covariance from an unoccupied voxel");
  }
  return covariance;
}

const Eigen::Vector3d & StaticNDTVoxel::centroid() const
{
  if (!m_occupied) {
    throw std::out_of_range("StaticNDTVoxel: Cannot get centroid from an unoccupied voxel");
  }
  return m_centroid;
}

const Eigen::Matrix3d & StaticNDTVoxel::inverse_covariance() const
{
  if (!m_occupied) {
    throw std::out_of_range(
            "StaticNDTVoxel: Cannot get inverse covariance "
            "from an unoccupied voxel");
  }
  return m_inv_covariance;
}

bool8_t StaticNDTVoxel::usable() const noexcept
{
  return m_occupied;
}
}  // namespace ndt
}  // namespace localization
}  // namespace autoware