//---------------------------------------------------------------------------//
//!
//! \file   tstExponentialDistribution.cpp
//! \author Alex Robinson
//! \brief  Histogram distribution unit tests.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>
#include <limits>

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

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_ParameterList.hpp>
#include <Teuchos_XMLParameterListCoreHelpers.hpp>
#include <Teuchos_VerboseObject.hpp>

// FRENSIE Includes
#include "Utility_UnitTestHarnessExtensions.hpp"
#include "Utility_OneDDistribution.hpp"
#include "Utility_ExponentialDistribution.hpp"
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_ElectronVoltUnit.hpp"

using boost::units::quantity;
using namespace Utility::Units;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

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

Teuchos::RCP<Teuchos::ParameterList> test_dists_list;

Teuchos::RCP<Utility::OneDDistribution> distribution(
			     new Utility::ExponentialDistribution( 2.0, 3.0 ) );
Teuchos::RCP<Utility::UnitAwareOneDDistribution<cgs::length,si::amount> > unit_aware_distribution( new Utility::UnitAwareExponentialDistribution<cgs::length,si::amount>( 2.0*si::mole, 300.0/si::meter, 0.0*si::meter ) );

//---------------------------------------------------------------------------//
// Tests.
//---------------------------------------------------------------------------//
// Check that the distribution can be evaluated
TEUCHOS_UNIT_TEST( ExponentialDistribution, evaluate )
{
  TEST_EQUALITY_CONST( distribution->evaluate( -1.0 ), 0.0 );
  TEST_EQUALITY_CONST( distribution->evaluate( 0.0 ), 2.0 );
  TEST_FLOATING_EQUALITY(distribution->evaluate( 1.0 ), 2.0*exp(-3.0), 1e-12);
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be evaluated
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, evaluate )
{
  TEST_EQUALITY_CONST( unit_aware_distribution->evaluate(-1.0*cgs::centimeter),
		       0.0*si::mole );
  TEST_EQUALITY_CONST( unit_aware_distribution->evaluate( 0.0*cgs::centimeter),
		       2.0*si::mole );
  UTILITY_TEST_FLOATING_EQUALITY(
		       unit_aware_distribution->evaluate( 1.0*cgs::centimeter),
		       2.0*exp(-3.0)*si::mole,
		       1e-12 );
}

//---------------------------------------------------------------------------//
// Check that the PDF can be evaluated
TEUCHOS_UNIT_TEST( ExponentialDistribution, evaluatePDF )
{
  TEST_EQUALITY_CONST( distribution->evaluatePDF( -1.0 ), 0.0 );
  TEST_EQUALITY_CONST( distribution->evaluatePDF( 0.0 ), 3.0 );
  TEST_FLOATING_EQUALITY(distribution->evaluatePDF(1.0), 3.0*exp(-3.0), 1e-12);
}

//---------------------------------------------------------------------------//
// Check that the unit-aware PDF can be evaluated
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, evaluatePDF )
{
  TEST_EQUALITY_CONST(
		  unit_aware_distribution->evaluatePDF( -1.0*cgs::centimeter ),
		  0.0/cgs::centimeter );
  TEST_EQUALITY_CONST(
		   unit_aware_distribution->evaluatePDF( 0.0*cgs::centimeter ),
		   3.0/cgs::centimeter );
  UTILITY_TEST_FLOATING_EQUALITY(
		   unit_aware_distribution->evaluatePDF( 1.0*cgs::centimeter ),
		   3.0*exp(-3.0)/cgs::centimeter,
		   1e-12 );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
TEUCHOS_UNIT_TEST( ExponentialDistribution, sample_basic_static )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  double sample = Utility::ExponentialDistribution::sample( 3.0 );
  TEST_EQUALITY_CONST( sample, 0.0 );

  sample = Utility::ExponentialDistribution::sample( 3.0 );
  TEST_FLOATING_EQUALITY( sample, 11.5131919974469596, 1e-15 );

  sample = Utility::ExponentialDistribution::sample( 3.0 );
  TEST_FLOATING_EQUALITY( sample, -log(0.5)/3.0, 1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, sample_basic_static )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  quantity<cgs::length> sample =
    Utility::UnitAwareExponentialDistribution<cgs::length>::sample( 3.0/cgs::centimeter );
  TEST_EQUALITY_CONST( sample, 0.0*cgs::centimeter );

  sample = Utility::UnitAwareExponentialDistribution<cgs::length>::sample( 3.0/cgs::centimeter );
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  11.5131919974469596*cgs::centimeter,
				  1e-15 );

  sample = Utility::UnitAwareExponentialDistribution<cgs::length>::sample( 3.0/cgs::centimeter );
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  -log(0.5)/3.0*cgs::centimeter,
				  1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
TEUCHOS_UNIT_TEST( ExponentialDistribution, sample_static )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  double inf = std::numeric_limits<double>::infinity();

  double sample = Utility::ExponentialDistribution::sample( 3.0, 0.0, inf );
  TEST_EQUALITY_CONST( sample, 0.0 );

  sample = Utility::ExponentialDistribution::sample( 3.0, 0.0, inf );
  TEST_FLOATING_EQUALITY( sample, 11.5131919974469596, 1e-15 );

  sample = Utility::ExponentialDistribution::sample( 3.0, 0.0, inf );
  TEST_FLOATING_EQUALITY( sample, -log(0.5)/3.0, 1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, sample_static )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  quantity<cgs::length> inf =
    Utility::QuantityTraits<quantity<cgs::length> >::inf();

  quantity<cgs::length> sample =
    Utility::UnitAwareExponentialDistribution<cgs::length>::sample(
			       3.0/cgs::centimeter, 0.0*cgs::centimeter, inf );
  TEST_EQUALITY_CONST( sample, 0.0*cgs::centimeter );

  sample = Utility::UnitAwareExponentialDistribution<cgs::length>::sample(
			       3.0/cgs::centimeter, 0.0*cgs::centimeter, inf );
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  11.5131919974469596*cgs::centimeter,
				  1e-15 );

  sample = Utility::UnitAwareExponentialDistribution<cgs::length>::sample(
			       3.0/cgs::centimeter, 0.0*cgs::centimeter, inf );
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  -log(0.5)/3.0*cgs::centimeter,
				  1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
TEUCHOS_UNIT_TEST( ExponentialDistribution, sample )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  double sample = distribution->sample();
  TEST_EQUALITY_CONST( sample, 0.0 );

  sample = distribution->sample();
  TEST_FLOATING_EQUALITY( sample, 11.5131919974469596, 1e-15 );

  sample = distribution->sample();
  TEST_FLOATING_EQUALITY( sample, -log(0.5)/3.0, 1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, sample )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  quantity<cgs::length> sample = unit_aware_distribution->sample();
  TEST_EQUALITY_CONST( sample, 0.0*cgs::centimeter );

  sample = unit_aware_distribution->sample();
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  11.5131919974469596*cgs::centimeter,
				  1e-15 );

  sample = unit_aware_distribution->sample();
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  -log(0.5)/3.0*cgs::centimeter,
				  1e-12 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
TEUCHOS_UNIT_TEST( ExponentialDistribution, sampleAndRecordTrials )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  unsigned trials = 0;

  double sample = distribution->sampleAndRecordTrials( trials );
  TEST_EQUALITY_CONST( sample, 0.0 );
  TEST_EQUALITY_CONST( trials, 1 );

  sample = distribution->sampleAndRecordTrials( trials );
  UTILITY_TEST_FLOATING_EQUALITY( sample, 11.5131919974469596, 1e-15 );
  TEST_EQUALITY_CONST( trials, 2 );

  sample = distribution->sampleAndRecordTrials( trials );
  TEST_FLOATING_EQUALITY( sample, -log(0.5)/3.0, 1e-12 );
  TEST_EQUALITY_CONST( trials, 3 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, sampleAndRecordTrials )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 1.0 - 1e-15;
  fake_stream[2] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  unsigned trials = 0;

  quantity<cgs::length> sample =
    unit_aware_distribution->sampleAndRecordTrials( trials );
  TEST_EQUALITY_CONST( sample, 0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( trials, 1 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  11.5131919974469596*cgs::centimeter,
				  1e-15 );
  TEST_EQUALITY_CONST( trials, 2 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  UTILITY_TEST_FLOATING_EQUALITY( sample,
				  -log(0.5)/3.0*cgs::centimeter,
				  1e-12 );
  TEST_EQUALITY_CONST( trials, 3 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

//---------------------------------------------------------------------------//
// Check that the upper bound of the distribution independent variable can be
// returned
TEUCHOS_UNIT_TEST( ExponentialDistribution, getUpperBoundOfIndepVar )
{
  TEST_EQUALITY_CONST( distribution->getUpperBoundOfIndepVar(),
		       std::numeric_limits<double>::infinity() );
}

//---------------------------------------------------------------------------//
// Check that the upper bound of the unit-aware distribution independent
// variable can be returned
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, getUpperBoundOfIndepVar )
{
  TEST_EQUALITY_CONST( unit_aware_distribution->getUpperBoundOfIndepVar(),
		       Utility::QuantityTraits<quantity<cgs::length> >::inf());
}

//---------------------------------------------------------------------------//
// Check that the lower bound of the distribution independent variable can be
// returned
TEUCHOS_UNIT_TEST( ExponentialDistribution, getLowerBoundOfIndepVar )
{
  TEST_EQUALITY_CONST( distribution->getLowerBoundOfIndepVar(), 0.0 );
}

//---------------------------------------------------------------------------//
// Check that the lower bound of the unit-aware distribution independent
// variable can be returned
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, getLowerBoundOfIndepVar )
{
  TEST_EQUALITY_CONST( unit_aware_distribution->getLowerBoundOfIndepVar(),
		       Utility::QuantityTraits<quantity<cgs::length> >::zero());
}

//---------------------------------------------------------------------------//
// Check that the distribution type can be returned
TEUCHOS_UNIT_TEST( ExponentialDistribution, getDistributionType )
{
  TEST_EQUALITY_CONST( distribution->getDistributionType(),
		       Utility::EXPONENTIAL_DISTRIBUTION );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution type can be returned
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, getDistributionType )
{
  TEST_EQUALITY_CONST( unit_aware_distribution->getDistributionType(),
		       Utility::EXPONENTIAL_DISTRIBUTION );
}

//---------------------------------------------------------------------------//
// Check if the distribution is tabular
TEUCHOS_UNIT_TEST( ExponentialDistribution, isTabular )
{
  TEST_ASSERT( !distribution->isTabular() );
}

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is tabular
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, isTabular )
{
  TEST_ASSERT( !unit_aware_distribution->isTabular() );
}

//---------------------------------------------------------------------------//
// Check if the distribution is continuous
TEUCHOS_UNIT_TEST( ExponentialDistribution, isContinuous )
{
  TEST_ASSERT( distribution->isContinuous() );
}

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is continuous
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, isContinuous )
{
  TEST_ASSERT( unit_aware_distribution->isContinuous() );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be written to an xml file
TEUCHOS_UNIT_TEST( ExponentialDistribution, toParameterList )
{
  Teuchos::RCP<Utility::ExponentialDistribution> true_distribution =
   Teuchos::rcp_dynamic_cast<Utility::ExponentialDistribution>( distribution );

  Teuchos::ParameterList parameter_list;

  parameter_list.set<Utility::ExponentialDistribution>( "test distribution",
						     *true_distribution );

  Teuchos::writeParameterListToXmlFile( parameter_list,
					"exponential_dist_test_list.xml" );

  Teuchos::RCP<Teuchos::ParameterList> read_parameter_list =
    Teuchos::getParametersFromXmlFile( "exponential_dist_test_list.xml" );

  TEST_EQUALITY( parameter_list, *read_parameter_list );

  Teuchos::RCP<Utility::ExponentialDistribution>
    copy_distribution( new Utility::ExponentialDistribution );

  *copy_distribution =
    read_parameter_list->get<Utility::ExponentialDistribution>(
							  "test distribution");

  TEST_EQUALITY( *copy_distribution, *true_distribution );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be written to an xml file
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, toParameterList )
{
  typedef Utility::UnitAwareExponentialDistribution<cgs::length,si::amount>
    UnitAwareExponentialDistribution;

  Teuchos::RCP<UnitAwareExponentialDistribution> true_distribution =
   Teuchos::rcp_dynamic_cast<UnitAwareExponentialDistribution>(
						     unit_aware_distribution );

  Teuchos::ParameterList parameter_list;

  parameter_list.set<UnitAwareExponentialDistribution>( "test distribution",
							*true_distribution );

  Teuchos::writeParameterListToXmlFile( parameter_list,
					"unit_aware_exponential_dist_test_list.xml" );

  Teuchos::RCP<Teuchos::ParameterList> read_parameter_list =
    Teuchos::getParametersFromXmlFile( "unit_aware_exponential_dist_test_list.xml" );

  TEST_EQUALITY( parameter_list, *read_parameter_list );

  Teuchos::RCP<UnitAwareExponentialDistribution>
    copy_distribution( new UnitAwareExponentialDistribution );

  *copy_distribution =
    read_parameter_list->get<UnitAwareExponentialDistribution>(
							  "test distribution");

  TEST_EQUALITY( *copy_distribution, *true_distribution );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be read from an xml file
TEUCHOS_UNIT_TEST( ExponentialDistribution, fromParameterList )
{
  Utility::ExponentialDistribution read_distribution =
    test_dists_list->get<Utility::ExponentialDistribution>( "Exponential Distribution A" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0 ), 1.0 );
  TEST_FLOATING_EQUALITY( read_distribution.evaluate( 1.0 ),
			  exp( -3.0 ),
			  1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
		       std::numeric_limits<double>::infinity() );
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(), 0.0 );

  read_distribution =
    test_dists_list->get<Utility::ExponentialDistribution>( "Exponential Distribution B" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0 ),
		       Utility::PhysicalConstants::pi );
  TEST_FLOATING_EQUALITY( read_distribution.evaluate( 1.0 ),
			  Utility::PhysicalConstants::pi*exp( -3.0 ),
			  1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
		       std::numeric_limits<double>::infinity() );
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(), 0.0 );

  read_distribution =
    test_dists_list->get<Utility::ExponentialDistribution>( "Exponential Distribution C" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0 ), 0.0 );
  TEST_FLOATING_EQUALITY( read_distribution.evaluate( 1.0 ),
			  Utility::PhysicalConstants::pi*exp( -3.0 ),
			  1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
		       std::numeric_limits<double>::infinity() );
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(), 1.0 );

  read_distribution =
    test_dists_list->get<Utility::ExponentialDistribution>( "Exponential Distribution D" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0 ), 0.0 );
  TEST_FLOATING_EQUALITY( read_distribution.evaluate( 1.0 ),
			  Utility::PhysicalConstants::pi*exp( -3.0 ),
			  1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(), 2.0 );
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(), 1.0 );

  read_distribution =
    test_dists_list->get<Utility::ExponentialDistribution>( "Exponential Distribution E" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0 ), 0.0 );
  TEST_FLOATING_EQUALITY( read_distribution.evaluate( 1.0 ),
			  Utility::PhysicalConstants::pi*exp( -3.0 ),
			  1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
		       std::numeric_limits<double>::infinity() );
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(), 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be read from an xml file
TEUCHOS_UNIT_TEST( UnitAwareExponentialDistribution, fromParameterList )
{
  typedef Utility::UnitAwareExponentialDistribution<cgs::length,si::amount>
    UnitAwareExponentialDistribution;

  UnitAwareExponentialDistribution read_distribution =
    test_dists_list->get<UnitAwareExponentialDistribution>( "Unit-Aware Exponential Distribution A" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0*cgs::centimeter ),
  		       1.0*si::mole );
  UTILITY_TEST_FLOATING_EQUALITY(
  			     read_distribution.evaluate( 1.0*cgs::centimeter ),
  			     exp( -3.0 )*si::mole,
  			     1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
  		       Utility::QuantityTraits<quantity<cgs::length> >::inf());
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(),
  		       0.0*cgs::centimeter );

  read_distribution =
    test_dists_list->get<UnitAwareExponentialDistribution>(
				     "Unit-Aware Exponential Distribution B" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0*cgs::centimeter ),
  		       Utility::PhysicalConstants::pi*si::mole );
  UTILITY_TEST_FLOATING_EQUALITY(
			  read_distribution.evaluate( 1.0*cgs::centimeter ),
  			  Utility::PhysicalConstants::pi*exp( -3.0 )*si::mole,
  			  1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
  		       Utility::QuantityTraits<quantity<cgs::length> >::inf());
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );

  read_distribution =
    test_dists_list->get<UnitAwareExponentialDistribution>(
				     "Unit-Aware Exponential Distribution C" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0*cgs::centimeter ),
		       0.0*si::mole );
  UTILITY_TEST_FLOATING_EQUALITY(
			   read_distribution.evaluate( 1.0*cgs::centimeter ),
			   Utility::PhysicalConstants::pi*exp( -3.0 )*si::mole,
			   1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
  		       Utility::QuantityTraits<quantity<cgs::length> >::inf());
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(),
		       1.0*cgs::centimeter );

  read_distribution =
    test_dists_list->get<UnitAwareExponentialDistribution>(
				     "Unit-Aware Exponential Distribution D" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0*cgs::centimeter ),
		       0.0*si::mole );
  UTILITY_TEST_FLOATING_EQUALITY(
			   read_distribution.evaluate( 1.0*cgs::centimeter ),
			   Utility::PhysicalConstants::pi*exp( -3.0 )*si::mole,
			   1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
		       2.0*cgs::centimeter );
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(),
		       1.0*cgs::centimeter );

  read_distribution =
    test_dists_list->get<UnitAwareExponentialDistribution>(
				     "Unit-Aware Exponential Distribution E" );

  TEST_EQUALITY_CONST( read_distribution.evaluate( 0.0*cgs::centimeter ),
		       0.0*si::mole );
  UTILITY_TEST_FLOATING_EQUALITY(
			   read_distribution.evaluate( 1.0*cgs::centimeter ),
			   Utility::PhysicalConstants::pi*exp( -3.0 )*si::mole,
			   1e-15 );
  TEST_EQUALITY_CONST( read_distribution.getUpperBoundOfIndepVar(),
  		       Utility::QuantityTraits<quantity<cgs::length> >::inf());
  TEST_EQUALITY_CONST( read_distribution.getLowerBoundOfIndepVar(),
		       1.0*cgs::centimeter );
}

//---------------------------------------------------------------------------//
// Check that distributions can be scaled
TEUCHOS_UNIT_TEST_TEMPLATE_4_DECL( UnitAwareExponentialDistribution,
				   explicit_conversion,
				   IndepUnitA,
				   DepUnitA,
				   IndepUnitB,
				   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;

  // Copy from unitless distribution to distribution type A (static method)
  Utility::UnitAwareExponentialDistribution<IndepUnitA,DepUnitA>
    unit_aware_dist_a_copy = Utility::UnitAwareExponentialDistribution<IndepUnitA,DepUnitA>::fromUnitlessDistribution( *Teuchos::rcp_dynamic_cast<Utility::ExponentialDistribution>( distribution ) );

  // Copy from distribution type A to distribution type B (explicit cast)
  Utility::UnitAwareExponentialDistribution<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( 3.0 );
  DepQuantityA dep_quantity_a =
    Utility::QuantityTraits<DepQuantityA>::initializeQuantity( 2.0 );

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

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

  Utility::setQuantity( indep_quantity_a, 1.0 );
  Utility::setQuantity( inv_indep_quantity_a, 3.0*exp(-3.0) );
  Utility::setQuantity( dep_quantity_a, 2.0*exp(-3.0) );

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

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

typedef si::energy si_energy;
typedef cgs::energy cgs_energy;
typedef si::amount si_amount;
typedef si::length si_length;
typedef cgs::length cgs_length;
typedef si::mass si_mass;
typedef cgs::mass cgs_mass;
typedef si::dimensionless si_dimensionless;
typedef cgs::dimensionless cgs_dimensionless;

TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      si_energy,
				      si_amount,
				      cgs_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      cgs_energy,
				      si_amount,
				      si_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      si_energy,
				      si_length,
				      cgs_energy,
				      cgs_length );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      cgs_energy,
				      cgs_length,
				      si_energy,
				      si_length );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      si_energy,
				      si_mass,
				      cgs_energy,
				      cgs_mass );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      cgs_energy,
				      cgs_mass,
				      si_energy,
				      si_mass );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      si_energy,
				      si_dimensionless,
				      cgs_energy,
				      cgs_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      cgs_energy,
				      cgs_dimensionless,
				      si_energy,
				      si_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      si_energy,
				      void,
				      cgs_energy,
				      void );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      cgs_energy,
				      void,
				      si_energy,
				      void );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      ElectronVolt,
				      si_amount,
				      si_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      ElectronVolt,
				      si_amount,
				      cgs_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      ElectronVolt,
				      si_amount,
				      KiloElectronVolt,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      ElectronVolt,
				      si_amount,
				      MegaElectronVolt,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      KiloElectronVolt,
				      si_amount,
				      si_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      KiloElectronVolt,
				      si_amount,
				      cgs_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      KiloElectronVolt,
				      si_amount,
				      ElectronVolt,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      KiloElectronVolt,
				      si_amount,
				      MegaElectronVolt,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      MegaElectronVolt,
				      si_amount,
				      si_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      MegaElectronVolt,
				      si_amount,
				      cgs_energy,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      MegaElectronVolt,
				      si_amount,
				      ElectronVolt,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      MegaElectronVolt,
				      si_amount,
				      KiloElectronVolt,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwareExponentialDistribution,
				      explicit_conversion,
				      void,
				      MegaElectronVolt,
				      void,
				      KiloElectronVolt );

//---------------------------------------------------------------------------//
// Custom main function
//---------------------------------------------------------------------------//
int main( int argc, char** argv )
{
  std::string test_dists_xml_file;

  Teuchos::CommandLineProcessor& clp = Teuchos::UnitTestRepository::getCLP();

  clp.setOption( "test_dists_xml_file",
		 &test_dists_xml_file,
		 "Test distributions xml file name" );

  const Teuchos::RCP<Teuchos::FancyOStream> out =
    Teuchos::VerboseObjectBase::getDefaultOStream();

  Teuchos::CommandLineProcessor::EParseCommandLineReturn parse_return =
    clp.parse(argc,argv);

  if ( parse_return != Teuchos::CommandLineProcessor::PARSE_SUCCESSFUL ) {
    *out << "\nEnd Result: TEST FAILED" << std::endl;
    return parse_return;
  }

  TEUCHOS_ADD_TYPE_CONVERTER( Utility::ExponentialDistribution );
  typedef Utility::UnitAwareExponentialDistribution<cgs::length,si::amount> UnitAwareExponentialDistribution;
  TEUCHOS_ADD_TYPE_CONVERTER( UnitAwareExponentialDistribution );

  test_dists_list = Teuchos::getParametersFromXmlFile( test_dists_xml_file );

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

  // Run the unit tests
  Teuchos::GlobalMPISession mpiSession( &argc, &argv );

  const bool success = Teuchos::UnitTestRepository::runUnitTests(*out);

  if (success)
    *out << "\nEnd Result: TEST PASSED" << std::endl;
  else
    *out << "\nEnd Result: TEST FAILED" << std::endl;

  clp.printFinalTimerSummary(out.ptr());

  return (success ? 0 : 1);
}

//---------------------------------------------------------------------------//
// end tstExponentialDistribution.cpp
//---------------------------------------------------------------------------//


#include "catch.hpp"

#include <blitzml/base/common.h>
#include <blitzml/base/vector_util.h>
#include <vector>
#include <algorithm>

using std::vector;

using namespace BlitzML;

TEST_CASE( "test_crude_shuffle", "[vector_util]" ) {
  vector<int> v(100, 0);
  for (size_t i = 0; i < v.size(); ++i) {
    v[i] = i;
  }
  crude_shuffle(v, 0, v.size());
  bool is_sorted = true;
  for (size_t i = 1; i < v.size(); ++i) {
    if (v[i] < v[i -1]) {
      is_sorted = false;
      break;
    }
  }
  REQUIRE( is_sorted == false );
}

TEST_CASE( "indirect_sort_indices", "[vector_util]" ) {
  vector<index_t> indices;
  indices.push_back(1);
  indices.push_back(0);
  indices.push_back(3);
  indices.push_back(2);
  vector<value_t> values;
  values.push_back(5.0);
  values.push_back(1.1);
  values.push_back(1.3);
  values.push_back(1.4);
  indirect_sort_indices(indices, values);
  REQUIRE( indices[0] == 1 );
  REQUIRE( indices[1] == 2 );
  REQUIRE( indices[2] == 3 );
  REQUIRE( indices[3] == 0 );
}

TEST_CASE( "scale_vector", "[vector_util]" ) {
  vector<double> v;
  v.push_back(2.0);
  v.push_back(-1.0);
  scale_vector(v, -2.0);
  REQUIRE( v[0] == Approx(-4.0) );
  REQUIRE( v[1] == Approx(2.0) );
  v.push_back(3.5);
  scale_vector(v, 0.5);
  REQUIRE( v[0] == Approx(-2.0) );
  REQUIRE( v[1] == Approx(1.0) );
  REQUIRE( v[2] == Approx(1.75) );

  add_scalar_to_vector(v, -0.5);
  REQUIRE( v[0] == Approx(-2.5) );
  REQUIRE( v[1] == Approx(0.5) );
  REQUIRE( v[2] == Approx(1.25) );
}

TEST_CASE( "is_vector_const", "[vector_util]" ) {
  vector<double> v(100, -25.0);
  REQUIRE( is_vector_const(v) == true );
  v[0] = 25.;
  REQUIRE( is_vector_const(v) == false );
  v[0] = -25.;
  REQUIRE( is_vector_const(v) == true );
  v[99] = 25.;
  REQUIRE( is_vector_const(v) == false );
}


// Copyright (C) 2018  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/optimization.h>
#include <dlib/global_optimization.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.isotonic_regression");

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

    class optimization_tester : public tester
    {
    public:
        optimization_tester (
        ) :
            tester ("test_isotonic_regression",
                    "Runs tests on the isotonic_regression object.")
        {}

        void perform_test (
        )
        {
            dlib::rand rnd;

            for (int round = 0; round < 100; ++round)
            {
                print_spinner();
                std::vector<double> vect;
                for (int i = 0; i < 5; ++i)
                    vect.push_back(put_in_range(-1,1,rnd.get_random_gaussian()));

                auto f = [&](const matrix<double,0,1>& x)
                {
                    double dist = 0;
                    double sum = 0;
                    for (long i = 0; i < x.size(); ++i)
                    {
                        sum += x(i);
                        dist += (sum-vect[i])*(sum-vect[i]);
                    }
                    return dist;
                };

                auto objval = [vect](const matrix<double,0,1>& x)
                {
                    return sum(squared(mat(vect)-x));
                };

                auto is_monotonic = [](const matrix<double,0,1>& x)
                {
                    for (long i = 1; i < x.size(); ++i)
                    {
                        if (x(i-1) > x(i))
                            return false;
                    }
                    return true;
                };

                matrix<double,0,1> lower(5), upper(5);
                lower = 0;
                lower(0) = -4;
                upper = 4;
                // find the solution with find_min_global() and then check that it matches
                auto result = find_min_global(f, lower, upper, max_function_calls(40));

                for (long i = 1; i < result.x.size(); ++i)
                    result.x(i) += result.x(i-1);

                isotonic_regression mr;
                mr(vect);

                dlog << LINFO << "err: "<<  objval(mat(vect)) - objval(result.x);

                DLIB_CASSERT(is_monotonic(mat(vect)));
                DLIB_CASSERT(is_monotonic(result.x));
                // isotonic_regression should be at least as good as find_min_global().
                DLIB_CASSERT(objval(mat(vect)) - objval(result.x) < 1e-13);
            }

        }
    } a;

}


/**
 * \file path/to/include/file.hpp
 *
 * \brief Rotate matrix 90 degrees.
 *
 * Inspired by the \c rot90 MATLAB function.
 * See http://www.mathworks.com/help/techdoc/ref/rot90.html.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_ROT90_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_ROT90_HPP

//#include <boost/numeric/ublasx/detail/temporary.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename MatrixT>
struct rot90_matrix_traits
{
//Not all matrix types have this type (e.g., scalar_matrix<>)
//	typedef typename detail::matrix_temporary_traits<MatrixT>::type result_type;
	typedef matrix<typename matrix_traits<MatrixT>::value_type> result_type;
};

template <typename VectorT>
struct rot90_vector_traits
{
//Not all vector types have this type (e.g., scalar_vector<>)
//	typedef typename detail::vector_temporary_traits<VectorT>::type result_type;
	typedef vector<typename vector_traits<VectorT>::value_type> result_type;
};

} // Namespace detail

/// Rotate the given vector counterclockwise by (a multiple of) 90 degrees.
template <typename VectorT>
typename detail::rot90_vector_traits<VectorT>::result_type rot90(vector_expression<VectorT> const& v, int k=1)
{
	typedef typename detail::rot90_vector_traits<VectorT>::result_type result_type;
	typedef typename vector_traits<VectorT>::size_type size_type;

	size_type n(ublasx::size(v));

	result_type x;

	// Make sure k \in {0, 1, 2, 3}
	k %= 4;
	if (k < 0)
	{
		k += 4;
	}

	// NOTE: uBLAS makes no distinction between row and column vector.
	//       So rotations for k=0 and k=1 are considered identical.
	//       The same applies for rotations for k=2 and k=3.

	if (k == 2 || k == 3)
	{
		x.resize(n, false);

		for (size_type i = 0; i < n; ++i)
		{
			x(n-i-1) = v()(i);
		}
	}
	else
	{
		x = v;
	}

	return x;
}

/// Rotate the given vector counterclockwise by (a multiple of) 90 degrees.
template <typename VectorT>
BOOST_UBLAS_INLINE
void rot90_inplace(vector_container<VectorT>& v, int k=1)
{
	v() = rot90(v, k);
}

/// Rotate the given matrix counterclockwise by (a multiple of) 90 degrees
template <typename MatrixT>
typename detail::rot90_matrix_traits<MatrixT>::result_type rot90(matrix_expression<MatrixT> const& A, int k=1)
{
	typedef typename detail::rot90_matrix_traits<MatrixT>::result_type result_type;
	typedef typename matrix_traits<MatrixT>::size_type size_type;

	size_type nr(ublasx::num_rows(A));
	size_type nc(ublasx::num_columns(A));

	result_type X;

	// Make sure k \in {0, 1, 2, 3}
	k %= 4;
	if (k < 0)
	{
		k += 4;
	}

	if (k == 1)
	{
		X.resize(nc, nr, false);

		for (size_type c = 0; c < nc; ++c)
		{
			row(X, nc-c-1) = column(A(), c);
		}
	}
	else if (k == 2)
	{
		X.resize(nr, nc, false);

		for (size_type r = 0; r < nr; ++r)
		{
			for (size_type c = 0; c < nc; ++c)
			{
				X(nr-r-1,nc-c-1) = A()(r,c);
			}
		}
	}
	else if (k == 3)
	{
		X.resize(nc, nr, false);

		for (size_type r = 0; r < nr; ++r)
		{
			column(X, nr-r-1) = row(A(), r);
		}
	}
	else
	{
		X = A;
	}

	return X;
}

/// Rotate the given matrix counterclockwise by (a multiple of) 90 degrees
template <typename MatrixT>
BOOST_UBLAS_INLINE
void rot90_inplace(matrix_container<MatrixT>& A, int k=1)
{
	A() = rot90(A, k);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_ROT90_HPP


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( test )
{
    double v1 = 1.111e-10;
    double v2 = 1.112e-10;

    BOOST_CHECK_CLOSE_FRACTION( v1, v2, 0.0008999 );
}

//____________________________________________________________________________//


#ifndef _COMPLEMENTARY_FILTER_HPP_
#define _COMPLEMENTARY_FILTER_HPP_

#include <cmath>
#include <stdio.h>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <boost/math/special_functions.hpp>

// #define LOCAL_MAG_X 225.1 //miligauss
// #define LOCAL_MAG_Y 5.75
// #define LOCAL_MAG_Z 415.2

constexpr double gravity = 9.80665;
constexpr double local_mx = 208.762; //in miligauss
constexpr double local_my = 3.448;
constexpr double local_mz = 434.129;

using namespace std;

    class ComplementaryFilter{

        // constructor
        public:

        ComplementaryFilter()
            {
                is_init = false;
                data_fused = false;
                filter_updated = false;
                magFieldEarth = Eigen::Vector3d(local_mx,local_my,local_mz);
                mag = Eigen::Vector3d(0,0,0);
                accel = Eigen::Vector3d(0,0,0);
                gyro = Eigen::Vector3d(0,0,0);
            }

        struct parameters
        {
            double k1;
            double k2;
            double k3;
            double k4;
            double kb;
            double sat;
        };

        // Maths Functions
        Eigen::Matrix3d quatToRotationMatrix(Eigen::Vector4d quat);  // transforme quaternion to rotation matrix

        // Filter Methods
        void initFilter();
        void updateSensorData(Eigen::Vector3d gyro, Eigen::Vector3d accel, Eigen::Vector3d mag);
        void fuseInertialData();
        void updateFilter();

        private:

        // filter variables;
        double dt;
        Eigen::Vector3d magFieldEarth;
        Eigen::Matrix3d rotationMatrix;
        Eigen::Vector3d mag;
        Eigen::Vector3d accel;
        Eigen::Vector3d gyro;
        Eigen::Vector3d bias;
        Eigen::Vector3d sigR;
        Eigen::Vector3d sigB;
        Eigen::Vector3d gyro_estimate;
        Eigen::Quaterniond quaternion;
        Eigen::Matrix3d rotation_matrix;

        // filter flags
        bool is_init;
        bool data_fused;
        bool filter_updated;

        // filter parameters
        parameters params;

    };

#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "videoray/Throttle.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/Float32.h"

#include <iostream>
#include <sstream>

#include <boost/numeric/odeint.hpp>

using std::cout;
using std::endl;

using namespace boost::numeric::odeint;

typedef boost::array< double , 12 > state_type;

#define PI (3.14159265359)

nav_msgs::Odometry odom_;
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
     odom_ = *msg;
}

state_type x_ = {0,0,0,0,0,0,0,0,0,0,0,0};

// Linear and angular velocity states
double u;
double v;
double w;
double p;
double q;
double r;

// Added Mass Terms
double X_udot = 1.94; // inertia matrix M (m11)
double Y_vdot = 6.05; // inertia matrix M (m22)
double Z_wdot = 3.95; // m33
double N_rdot = 0.1;
//double N_rdot = 1.18e-2; // vehicle's motion of inertia about z-axis
// (6,6) entry of the vehicle inertia Matrix M

// Linear Drag Coefficients
double Xu = -0.95;
double Yv = -5.87;
double Nr = -0.023;
double Zw = -3.70;

// Quadratic Drag Coefficients
double Xuu = -6.04;
double Yvv = -30.73;
double Nrr = -0.45;
double Zww = -26.36;

double Ct_forw = 0.026667;
double Ct_back = 0.026667;
double Ct_vert_forw = 0.026667;
double Ct_vert_back = 0.026667;

double u_sat_low = -150;
double u_sat_high = 150;

// Control inputs
double X = 0;
double N = 0;
double Z = 0;

double u_port = 0;
double u_star = 0;
double u_vert = 0;

double xpos  = 0;
double ypos  = 0;
double zpos  = 0;
double phi   = 0;
double theta = 0;
double psi   = 0;

double c1 = 0;
double c2 = 0;
double c3 = 0;
double s1 = 0;
double s2 = 0;
double s3 = 0;
double t2 = 0;

videoray::Throttle throttle_;

void videoray_model( const state_type &x , state_type &dxdt , double t )
{
/// States:
/// 0:  u     : surge velocity
/// 1:  v     : sway velocity
/// 2:  w     : heave velocity
/// 3:  p     : roll rate
/// 4:  q     : pitch rate
/// 5:  r     : yaw rate
/// 6:  xpos  : earth x-pos
/// 7:  ypos  : earth y-pos
/// 8:  zpos  : earth z-pos
/// 9:  phi   : roll angle
/// 10: theta : pitch angle
/// 11: psi   : yaw angle
     u = x[0];
     v = x[1];
     w = x[2];
     p = x[3];
     q = x[4];
     r = x[5];
     xpos  = x[6];
     ypos  = x[7];
     zpos  = x[8];
     phi   = x[9];
     theta = x[10];
     psi   = x[11];

     // Calculate fixed frame velocity rates
     dxdt[0] = (-Y_vdot*v*r + Xu*u + Xuu*u*abs(u) + X) / X_udot;
     dxdt[1] = (X_udot*u*r + Yv*v + Yvv*v*abs(v)) / Y_vdot;
     dxdt[2] = (Zw*w + Zww*w*abs(w) + Z) / Z_wdot;

     // Calculate fixed frame orientation rates
     dxdt[3] = 0;
     dxdt[4] = 0;
     dxdt[5] = (Nr*r + Nrr*r*abs(r) + N) / N_rdot;

     c1 = cos(phi);
     c2 = cos(theta);
     c3 = cos(psi);
     s1 = sin(phi);
     s2 = sin(theta);
     s3 = sin(psi);
     t2 = tan(theta);

     // Calculate inertial frame position
     dxdt[6] = c3*c2*u + (c3*s2*s1-s3*c1)*v + (s3*s1+c3*c1*s2)*w;
     dxdt[7] = s3*c2*u + (c1*c3+s1*s2*s3)*v + (c1*s2*s3-c3*s1)*w;
     dxdt[8] = -s2*u + c2*s1*v + c1*c2*w;

     // Calculate inertial frame orientations
     dxdt[9] = p + (q*s1 + r*c1)*t2;
     dxdt[10] = q*c1 - r*s1;
     dxdt[11] = (q*s1 + r*c1)* (1 / cos(theta));
}

//
// Converts input throttle commands to simulated linear and angular velocities.
//
geometry_msgs::Twist velocity_cmd_;
geometry_msgs::Vector3 velocity_linear_;
geometry_msgs::Vector3 velocity_angular_;

double saturate(double input, const double &min, const double &max)
{
     if (min > max) {
          ROS_INFO("saturate(): Invalid Min / Max Combo");
          return 0;
     } else if (input < min) {
          input = min;
     } else if(input > max) {
          input = max;
     }
     return input;
}

// Assumes that input has already been saturated within the in_min and in_max
// boundaries. Use the saturate() function on input before calling normalize
double normalize(double input, const double &in_min, const double &in_max,
                 const double &out_min, const double &out_max)
{
     if (in_min >= in_max || out_min >= out_max) {
          ROS_INFO("normalize(): Invalid Min / Max Combo");
          return 0;
     }

     double ratio = input / (in_max - in_min);
     return ratio * (out_max - out_min);

     return input;
}

double thrust_port = 0, thrust_star = 0;
void processThrottleCmds()
{
     u_port = saturate(throttle_.PortInput, u_sat_low, u_sat_high);
     u_star = saturate(throttle_.StarInput, u_sat_low, u_sat_high);
     u_vert = saturate(throttle_.VertInput, u_sat_low, u_sat_high);

     // Ct is different for reverse and forward
     if ( u_port >= 0 ) {
          thrust_port = u_port * Ct_forw;
     } else {
          thrust_port = u_port * Ct_back;
     }

     if ( u_star >= 0 ) {
          thrust_star = u_star * Ct_forw;
     } else {
          thrust_star = u_star * Ct_back;
     }

     X = thrust_port + thrust_star;
     N = thrust_star - thrust_port;

     // Ct is different for reverse and forward
     if ( u_vert >= 0 ) {
          Z = u_vert * Ct_vert_forw;
     } else {
          Z = u_vert * Ct_vert_back;
     }

}

double normDegrees(double input)
{
     if (input < 0) {
          input += 360;
     } else if(input >= 360) {
          input -= 360;
     }
     return input;
}

void quaternionToEuler(const double &q0, const double &q1,
                       const double &q2, const double &q3,
                       double &roll, double &pitch, double &yaw)
{
     roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2) );
     pitch = asin(2*(q0*q2-q3*q1));
     yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3) );
}

void eulerToQuaternion(const double &roll, const double &pitch,
                       const double &yaw,
                       double &q0, double &q1,
                       double &q2, double &q3)
{
     q0 = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2);
     q1 = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2);
     q2 = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2);
     q3 = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2);
}

double depth_ref = 0;
double speed_ref = 0;
double heading_ref = 0;

void desiredVelocityCallback(const std_msgs::Float32::ConstPtr& msg)
{
     speed_ref = msg->data;
}

void desiredHeadingCallback(const std_msgs::Float32::ConstPtr& msg)
{
     heading_ref = normDegrees(msg->data - 90);
     //heading_ref = msg->data;
}

void desiredDepthCallback(const std_msgs::Float32::ConstPtr& msg)
{
     depth_ref = msg->data;
}

//geometry_msgs::Quaternion quat_;
double depth_err = 0;
double speed_err = 0;
double heading_err = 0;
double heading = 0;
double heading_port, heading_star, speed_port, speed_star;
double heading_weight = 0.5;
double speed_weight = 0.5;
//double K_heading = 0.25;
double K_heading = 0.01;
double K_speed = 10;
double K_depth = 50;
double roll_ = 0;
double pitch_ = 0;
double yaw_ = 0;

void execControlLaw()
{
     //quat_ = odom_.pose.pose.orientation;
     //quaternionToEuler(quat_.w, quat_.x, quat_.y, quat_.z,
     //                  roll_, pitch_, yaw_);

     //depth_err = depth_ref - odom_.pose.pose.position.z;
     //speed_err = speed_ref - odom_.twist.twist.linear.x;

     roll_ = x_[9];
     pitch_ = x_[10];
     yaw_ = x_[11];

     depth_err = depth_ref - x_[8];
     speed_err = speed_ref - x_[0];

     heading = normDegrees(yaw_*180/PI);
     heading_err = heading_ref - heading;

     if (abs(heading_err) < 180) {
          heading_port = -K_heading*heading_err;
          heading_star = K_heading*heading_err;
     } else  {
          heading_port = K_heading*heading_err;
          heading_star = -K_heading*heading_err;
     }

     speed_port = K_speed*speed_err;
     speed_star = K_speed*speed_err;

     throttle_.PortInput = heading_weight*heading_port + speed_weight*speed_port;
     throttle_.StarInput = heading_weight*heading_star + speed_weight*speed_star;
     throttle_.VertInput = K_depth*depth_err;

     //throttle_.PortInput = 100;
     //throttle_.StarInput = 95;
     //throttle_.VertInput = 0;
}

int main(int argc, char **argv)
{
     ros::init(argc, argv, "videoray_sim_and_control");
     ros::NodeHandle n;

     //ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("motion", 1);
     //ros::Subscriber odom_sub = n.subscribe("odometry", 1,
     //                                       odomCallback);

     ros::Publisher pose_pub = n.advertise<geometry_msgs::Pose>("motion",1);
     geometry_msgs::Pose pose_;

     ros::Publisher pub_nav_x = n.advertise<std_msgs::Float32>("NAV_X",1);
     ros::Publisher pub_nav_y = n.advertise<std_msgs::Float32>("NAV_Y",1);
     ros::Publisher pub_nav_depth = n.advertise<std_msgs::Float32>("NAV_DEPTH",1);
     ros::Publisher pub_nav_heading = n.advertise<std_msgs::Float32>("NAV_HEADING",1);
     ros::Publisher pub_nav_speed = n.advertise<std_msgs::Float32>("NAV_SPEED",1);

     std_msgs::Float32 nav_x, nav_y, nav_depth, nav_heading, nav_speed;

     ros::Subscriber desired_vel_sub = n.subscribe("desired_velocity",
                                                   1,
                                                   desiredVelocityCallback);

     ros::Subscriber desired_head_sub = n.subscribe("desired_heading",
                                                    1,
                                                    desiredHeadingCallback);

     ros::Subscriber desired_depth_sub = n.subscribe("desired_depth",
                                                     1,
                                                     desiredDepthCallback);

     //double rate = 30;
     double rate = 10;
     ros::Rate loop_rate(rate);

     ros::Time begin = ros::Time::now();
     ros::Time curr_time = begin;
     ros::Time prev_time = begin;
     ros::Duration dt = curr_time - prev_time;

     geometry_msgs::Quaternion quat;

     runge_kutta4< state_type > stepper;
     //runge_kutta_dopri5< state_type > stepper;
     //adams_bashforth_moulton< 2 , state_type > stepper;

     while (ros::ok())
     {
          //cout << "*" << std::flush;

          curr_time = ros::Time::now();
          dt = curr_time - prev_time;
          prev_time = curr_time;

          ROS_INFO("dt: %f\n", dt.toSec());

          //cout << dt.toSec() << endl << std::flush;

          // Update state vector with odometry data from morse...
          //state_type x = {0,0,0,0,0,0,0,0,0,0,0,0};
          //x[0] = odom_.twist.twist.linear.x;
          //x[1] = odom_.twist.twist.linear.y;
          //x[2] = odom_.twist.twist.linear.z;
          //x[3] = odom_.twist.twist.angular.x;
          //x[4] = odom_.twist.twist.angular.y;
          //x[5] = odom_.twist.twist.angular.z;

          execControlLaw();

          processThrottleCmds();

          //geometry_msgs::Quaternion quat = odom_.pose.pose.orientation;
          //quaternionToEuler(quat.x, quat.y, quat.z, quat.w,
          //                  roll_, pitch_, yaw_);

          //boost::numeric::odeint::integrate(videoray_model,
          //                                  x_,
          //                                  curr_time.toSec() ,
          //                                  (curr_time + dt).toSec(),
          //                                  dt.toSec());
          //boost::numeric::odeint::integrate(videoray_model,
          //                                  x_,
          //                                  curr_time.toSec() ,
          //                                  curr_time.toSec() + 1.0/rate,
          //                                  1.0/rate);

          stepper.do_step(videoray_model, x_ , curr_time.toSec() , dt.toSec() );

          ROS_INFO("========================");
          ROS_INFO("Current: %f, \tdt: %f", curr_time.toSec(), dt.toSec());
          ROS_INFO("Surge: %f", x_[0]);
          ROS_INFO("Sway: %f", x_[1]);
          ROS_INFO("Heave: %f", x_[2]);

          ROS_INFO("3: %f", x_[3]);
          ROS_INFO("4: %f", x_[4]);
          ROS_INFO("5: %f", x_[5]);
          ROS_INFO("6: %f", x_[6]);
          ROS_INFO("7: %f", x_[7]);
          ROS_INFO("8: %f", x_[8]);
          ROS_INFO("9: %f", x_[9]);
          ROS_INFO("10: %f", x_[10]);
          ROS_INFO("11: %f", x_[11]);

          //velocity_linear_.x = x[0];
          //velocity_linear_.y = x[1];
          //velocity_linear_.z = x[2];
          //velocity_angular_.z = x[5];
          //
          //velocity_cmd_.linear = velocity_linear_;
          //velocity_cmd_.angular = velocity_angular_;
          //twist_pub.publish(velocity_cmd_);

          //quaternionToEuler(quat.w, quat.x, quat.y, quat.z,
          //                  roll_, pitch_, yaw_);
          //
          //eulerToQuaternion(x_[9],
          //                  x_[10],
          //                  x_[11],
          //                  quat.w, quat.x, quat.y, quat.x);

          //quat.w = 0.1; quat.x = 0.2; quat.y = 0.3 ; quat.z = 0.4;

          //quaternionToEuler(quat.w, quat.x, quat.y, quat.z,
          //                  roll_, pitch_, yaw_);

          roll_  = x_[9];
          pitch_ = x_[10];
          yaw_   = x_[11];

          eulerToQuaternion(roll_,
                            pitch_,
                            yaw_,
                            quat.w, quat.x, quat.y, quat.z);

          pose_.position.x = x_[6];
          pose_.position.y = x_[7];
          pose_.position.z = x_[8];
          pose_.orientation.x = quat.x;
          pose_.orientation.y = quat.y;
          pose_.orientation.z = quat.z;
          pose_.orientation.w = quat.w;

          pose_pub.publish(pose_);

          nav_x.data = x_[6];
          nav_y.data = x_[7];
          nav_depth.data = x_[8];
          nav_speed.data = x_[0];
          nav_heading.data = normDegrees(x_[11]*180.0/PI + 90);

          pub_nav_x.publish(nav_x);
          pub_nav_y.publish(nav_y);
          pub_nav_depth.publish(nav_depth);
          pub_nav_heading.publish(nav_heading);
          pub_nav_speed.publish(nav_speed);

          ros::spinOnce();

          loop_rate.sleep();
     }
     return 0;
}


#include "NearestNeighbourSearch.hpp"

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
//#include <CGAL/point_generators_3.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/property_map.h>
#include <boost/iterator/zip_iterator.hpp>
#include <utility>
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point_3;
typedef boost::tuple<Point_3,int>                           Point_and_int;
//typedef CGAL::Random_points_in_cube_3<Point_3>              Random_points_iterator;
typedef CGAL::Search_traits_3<Kernel>                       Traits_base;
typedef CGAL::Search_traits_adapter<Point_and_int,
  CGAL::Nth_of_tuple_property_map<0, Point_and_int>,
  Traits_base>                                              Traits;
typedef CGAL::Orthogonal_k_neighbor_search<Traits>          K_neighbor_search;
typedef K_neighbor_search::Tree                             Tree;
typedef K_neighbor_search::Distance                         Distance;

  NearestNeighbourSearch::NearestNeighbourSearch(const std::vector<nniSample>& spoints)
    : m_sps(spoints),
      m_tree()
  {

    std::vector<Point_3>  points;
    std::vector<size_t>   indices;
    size_t idx = 0;
    for(const auto& sp : m_sps){
      points.push_back(Point_3(sp.s_pos.x, sp.s_pos.y, sp.s_pos.z));
      indices.push_back(idx);
      ++idx;
    }

    m_tree = new Tree(
		      boost::make_zip_iterator(boost::make_tuple( points.begin(),indices.begin() )),
		      boost::make_zip_iterator(boost::make_tuple( points.end(),indices.end() ) )
		      );

  }

  NearestNeighbourSearch::~NearestNeighbourSearch()
  {}

  std::vector<nniSample>
  NearestNeighbourSearch::search(const nniSample& ipolant,unsigned num_neighbours) const{
    std::vector<nniSample> result;

    Point_3 query(ipolant.s_pos.x, ipolant.s_pos.y, ipolant.s_pos.z);
    K_neighbor_search search(*(reinterpret_cast<Tree*>(m_tree)), query, num_neighbours);

    for(K_neighbor_search::iterator it = search.begin(); it != search.end(); it++){
      result.push_back(m_sps[boost::get<1>(it->first)]);
    }

    return result;
#if 0
    const unsigned int K = 5;
    Point_3 query(0.0, 0.0, 0.0);
    Distance tr_dist;
    // search K nearest neighbours
    K_neighbor_search search(*(reinterpret_cast<Tree*>(m_tree)), query, K);
    for(K_neighbor_search::iterator it = search.begin(); it != search.end(); it++){
      std::cout << " d(q, nearest neighbor) =  "
		<< tr_dist.inverse_of_transformed_distance(it->second) << " point: "
		<< boost::get<0>(it->first)<< " index " << boost::get<1>(it->first) << std::endl;
    }
#endif
    }


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

  @copyright 2015 NumScale SAS

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

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sqrt1pm1_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      A0 tmp =  bs::sqrt(inc(a0));
      return  ((bs::abs(a0) < bs::Half<A0>())? a0/bs::inc(tmp) : bs::dec(tmp));
    }
  };
} } }

#endif


#include <boost/graph/fruchterman_reingold.hpp>
#include <vector>

#include "graph_animator.hpp"

void GraphAnimator::add_vertices_edges(Graph &graph, GraphEventQueue &graph_event_queue, boost::gregorian::date date, boost::circle_topology<> topology)
{
    std::vector<Vertex> vertices = graph_event_queue.pop_vertices(date);
    std::vector<Edge> edges = graph_event_queue.pop_edges(date);

    // Add vertices.
    for (
        std::vector<Vertex>::iterator vertex_iterator = vertices.begin();
        vertex_iterator != vertices.end();
        ++vertex_iterator)
    {
        Vertex vertex = *vertex_iterator;
        vertex.position = topology.center();
        vertex.target_position = topology.center();

        boost::add_vertex(vertex.id, vertex, graph);
    }

    // Add edges.
    for (
        std::vector<Edge>::iterator edge_iterator = edges.begin();
        edge_iterator != edges.end();
        ++edge_iterator)
    {
        Edge edge = *edge_iterator;

        boost::add_edge_by_label(edge.source, edge.target, edge, graph);
    }
}

void GraphAnimator::evaluate_target_layout(Graph &graph, boost::circle_topology<> topology)
{
    boost::fruchterman_reingold_force_directed_layout(
        graph,
        boost::get(&Vertex::target_position, graph),
        topology,
        boost::force_pairs(boost::all_force_pairs()));
}

void GraphAnimator::update_layout(Graph &graph, boost::circle_topology<> topology, double step)
{
    VertexIterator vertex_iterator, vertex_iterator_end;
    for (
        boost::tie(vertex_iterator, vertex_iterator_end) = boost::vertices(graph);
        vertex_iterator != vertex_iterator_end;
        ++vertex_iterator)
    {
        Vertex *vertex = &graph.graph()[*vertex_iterator];
        vertex->position = topology.move_position_toward(vertex->position, step, vertex->target_position);
    }
}


/* test_uniform_int_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/uniform_int_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::uniform_int_distribution<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 0
#define BOOST_RANDOM_ARG2_DEFAULT 0x7fffffff
#define BOOST_RANDOM_ARG1_VALUE 100
#define BOOST_RANDOM_ARG2_VALUE 250

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX 0x7fffffff
#define BOOST_RANDOM_DIST1_MIN 100
#define BOOST_RANDOM_DIST1_MAX 0x7fffffff
#define BOOST_RANDOM_DIST2_MIN 100
#define BOOST_RANDOM_DIST2_MAX 250

#define BOOST_RANDOM_TEST1_PARAMS (0, 9)
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 9

#define BOOST_RANDOM_TEST2_PARAMS (10, 19)
#define BOOST_RANDOM_TEST2_MIN 10
#define BOOST_RANDOM_TEST2_MAX 19

#include "test_distribution.ipp"

#define BOOST_RANDOM_UNIFORM_INT boost::random::uniform_int_distribution

#include "test_uniform_int.ipp"


// -------------------------------------------------------------------
//
// basho_metrics: fast performance metrics for Erlang.
//
// inspired and partially derived from Coda Hale's 'metrics'
// Copyright (c) 2010-2001 Coda Hale
// https://github.com/codahale/metrics/blob/development/LICENSE.md
//
// Copyright (c) 2011 Basho Technologies, Inc. All Rights Reserved.
//
// This file is provided to you 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.
//
// -------------------------------------------------------------------
#ifndef SAMPLE_HPP_
#define SAMPLE_HPP_

#include <map>
#include <vector>
#include <cmath>
#include <ctime>
#include <cstdlib>
#include <stdint.h>
#include <sys/time.h>
#include <boost/circular_buffer.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

/**
 * An exponentially-decaying random sample of {@code long}s. Uses Cormode et
 * al's forward-decaying priority reservoir sampling method to produce a
 * statistically representative sample, exponentially biased towards newer
 * entries.
 *
 * @see
 * Cormode et al. Forward Decay: A Practical Time Decay Model for Streaming
 * Systems. ICDE '09: Proceedings of the 2009 IEEE International Conference on
 * Data Engineering (2009)
 */
template <typename IntType=unsigned long>
struct exponentially_decaying_sample
{
    exponentially_decaying_sample(std::size_t size, double alpha=0.015)
        : size_(size),
          alpha_(alpha),
          count_(0),
          start_time_(tick()),
          next_scale_time_(start_time_ + RESCALE_THRESHOLD)
    {
    }

    /**
     * Clears all recorded values.
     */
    void clear()
    {
        values_.clear();
        count_ = 0;
        start_time_ = tick();
        next_scale_time_ = start_time_ + RESCALE_THRESHOLD;
    }

   /**
     * Returns the number of values recorded.
     */
    std::size_t size() const
    {
        return std::min(size_, count_);
    }

    double weight(long t) const
    {
        return std::exp(alpha_ * t);
    }

    /**
     * Adds a new recorded value to the sample.
     */
    void update(IntType value)
    {
        update(value, tick());
    }

    void update(IntType value, long timestamp)
    {
        double priority = weight(timestamp - start_time_) / next_random();
        if (++count_ <= size_)
            values_[priority] = value;
        else
        {
            double first = values_.begin()->first;
            if (first < priority)
            {
                if (values_.find(priority) == values_.end() )
                    values_[priority] = value;
                values_.erase(values_.begin());
            }
        }
        long now = tick();
        if (now > next_scale_time_)
            rescale(now, next_scale_time_);
    }

    /* "A common feature of the above techniques—indeed, the key technique
     * that allows us to track the decayed weights efficiently—is that they
     * maintain counts and other quantities based on g(ti − L), and only scale
     * by g(t − L) at query time. But while g(ti −L)/g(t−L) is guaranteed to
     * lie between zero and one, the intermediate values of g(ti − L) could
     * become very large. For polynomial functions, these values should not
     * grow too large, and should be effectively represented in practice by
     * floating point values without loss of precision. For exponential
     * functions, these values could grow quite large as new values of (ti − L)
     * become large, and potentially exceed the capacity of common floating
     * point types. However, since the values stored by the algorithms are
     * linear combinations of g values (scaled sums), they can be rescaled
     * relative to a new landmark. That is, by the analysis of exponential
     * decay in Section III-A, the choice of L does not affect the final
     * result. We can therefore multiply each value based on L by a factor of
     * exp(−α(L′ − L)), and obtain the correct value as if we had instead
     * computed relative to a new landmark L′ (and then use this new L′ at
     * query time). This can be done with a linear pass over whatever data
     * structure is being used."
     */
    void rescale(long now, long next)
    {
        next_scale_time_ = now + RESCALE_THRESHOLD;
        long old_start_time = start_time_;
        start_time_ = tick();
        std::map<double, IntType> new_values;
        for (typename std::map<double, IntType>::const_iterator
             it=values_.begin();
             it != values_.end();
             ++it)
        {
            IntType value = it->second;
            new_values[it->first * std::exp(-alpha_ * (start_time_-old_start_time))] = value;
        }
        values_.swap(new_values);
    }

    /**
     * Returns a copy of the sample's values.
     */
    std::vector<IntType> values() const
    {
        std::vector<IntType> v;
        for (typename std::map<double, IntType>::const_iterator
                 it=values_.begin();
             it != values_.end();
             ++it)
        {
            v.push_back(it->second);
        }
        return v;
    }

private:
    long tick() const
    {
        return time(NULL);
    }

   double next_random()
   {
       return dist_(gen_) / static_cast<double>(std::numeric_limits<IntType>::max());
   }
private:
    std::size_t size_;
    double alpha_;
    std::size_t count_;
    long start_time_;
    long next_scale_time_;
    std::map<double, IntType> values_;
    boost::random::uniform_int_distribution<IntType> dist_;
    boost::random::mt19937 gen_;
    const static long RESCALE_THRESHOLD = 60;
};

/**
 * Sliding sample of a stream of {@code long}s. Operates on fixed-time window,
 * expired points are simply dropped.
 */
template <typename IntType=unsigned long>
struct sliding_sample
{
    sliding_sample(std::size_t size, std::size_t width_in_ms)
        : size_(size),
          width_(width_in_ms * 1000),
          ticks_(size),
          values_(size)
    {
    }

    typedef uint64_t tick_t;

public:
    void clear()
    {
        ticks_.clear();
        values_.clear();
    }

    std::size_t size() const
    {
        return values_.size();
    }

    void update(IntType value)
    {
        tick_t ts = tick();
        cut(ts);
        values_.push_back(value);
        ticks_.push_back(ts);
    }

    std::vector<IntType> values() const
    {
        std::size_t expired = std::min(expired_before(tick()), size() - 1);
        return std::vector<IntType>(values_.begin() + expired, values_.end());
    }

private:
    tick_t tick() const
    {
        timeval tv;
        if (0 == gettimeofday(&tv, 0)) {
            return uint64_t(tv.tv_sec) * 1000000 + tv.tv_usec;
        }
        return 0;
    }

    void cut(tick_t ts)
    {
        std::size_t cut = expired_before(ts);
        if (cut) {
            values_.erase(values_.begin(), values_.begin() + cut);
            ticks_.erase(ticks_.begin(), ticks_.begin() + cut);
        }
    }

    std::size_t expired_before(tick_t ts) const {
        tick_t cutoff = ts - width_;
        tick_buffer::const_iterator it = ticks_.begin(), end = ticks_.end();
        while (it != end && *it < cutoff) {
            ++it;
        }
        return it - ticks_.begin();
    }

private:
    std::size_t size_;
    tick_t width_;
    typedef boost::circular_buffer<tick_t> tick_buffer;
    tick_buffer ticks_;
    boost::circular_buffer<IntType> values_;

};

/**
 * A random sample of a stream of {@code long}s. Uses Vitter's Algorithm R to
 * produce a statistically representative sample.
 *
 * @see Random Sampling
 *      with a Reservoir
 */
template <typename IntType=unsigned long>
struct uniform_sample
{
    uniform_sample(std::size_t reservoir_size)
        : size_(reservoir_size),
          count_(0),
          values_(reservoir_size, 0)
    {
    }
public:
    void clear()
    {
        std::fill_n(values_.begin(), size_, 0);
    }

    std::size_t size() const
    {
        return std::min(count_, size_);
    }

    void update(IntType value)
    {
        std::size_t c = ++count_;
        if (c <= size_) { values_[c-1] = value; }
        else
        {
            std::size_t r = next_random() % c;
            if (r < size_) { values_[r] = value; }
        }
    }

    std::vector<IntType> values() const
    {
        return std::vector<IntType>(values_.begin(), values_.begin()+size());
    }

private:
    IntType next_random()
    {
        return dist_(gen_);
    }

private:
    std::size_t size_;
    std::size_t count_;
    std::vector<IntType> values_;
    boost::random::uniform_int_distribution<IntType> dist_;
    boost::random::mt19937 gen_;
};

#endif // include guard


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/Dual.h>
#include <CGAL/boost/graph/helpers.h>

#include <iostream>
#include <fstream>

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/connected_components.hpp>

typedef CGAL::Simple_cartesian<double>             Kernel;
typedef Kernel::Point_3                            Point;
typedef CGAL::Surface_mesh<Point>                  Mesh;
typedef CGAL::Dual<Mesh>                           Dual;
typedef boost::graph_traits<Dual>::edge_descriptor edge_descriptor;

template <typename G>
struct noborder {
  noborder() : g(nullptr) {} // default-constructor required by filtered_graph
  noborder(G& g) : g(&g) {}

  bool operator()(const edge_descriptor& e) const
  { return !is_border(e,*g); }

  G* g;
};

// A dual border edge has a null_face as the source or target "vertex"
// BGL algorithms won't like that, so we remove border edges through a
// boost::filtered_graph.
typedef boost::filtered_graph<Dual, noborder<Mesh> >   FiniteDual;
typedef boost::graph_traits<Mesh>::vertex_descriptor   vertex_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor     face_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor     edge_descriptor;

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/prim.off";

  Mesh primal;
  if(!CGAL::IO::read_polygon_mesh(filename, primal))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  Dual dual(primal);
  FiniteDual finite_dual(dual,noborder<Mesh>(primal));

  std::cout << "dual has " << num_vertices(dual) << " vertices" << std::endl;

  std::cout << "The vertices of dual are faces in primal"<< std::endl;
  for(boost::graph_traits<Dual>::vertex_descriptor dvd : vertices(dual)) {
    std::cout << dvd << std::endl;
  }

  std::cout << "The edges in primal and dual with source and target" << std::endl;
  for(edge_descriptor e : edges(dual)) {
   std::cout << e << " in primal:  " << source(e,primal)      << " -- " << target(e,primal)       << "   "
             <<      " in dual  :  " << source(e,finite_dual) << " -- " << target(e,finite_dual)  << std::endl;
  }

 std::cout << "edges of the finite dual graph" << std::endl;
 for(boost::graph_traits<FiniteDual>::edge_descriptor e : CGAL::make_range(edges(finite_dual))) {
   std::cout << e << "  " << source(e,primal) << " " << source(e,finite_dual)  << std::endl;
 }

 // the storage of a property map is in primal
 Mesh::Property_map<face_descriptor,int> fccmap;
 fccmap = primal.add_property_map<face_descriptor,int>("f:CC").first;
 int num = connected_components(finite_dual, fccmap);

 std::cout << "The graph has " << num << " connected components (face connectivity)" << std::endl;
 for(face_descriptor f : faces(primal)) {
   std::cout << f << " in connected component " << fccmap[f] << std::endl;
 }

 Mesh::Property_map<vertex_descriptor,int> vccmap;
 vccmap = primal.add_property_map<vertex_descriptor,int>("v:CC").first;
 num = connected_components(primal, vccmap);

 std::cout << "The graph has " << num << " connected components (edge connectvity)" << std::endl;
 for(vertex_descriptor v : vertices(primal)) {
   std::cout << v << " in connected component " << vccmap[v] << std::endl;
 }
  return 0;
}


#pragma once
#include <vector>
#include <Eigen/Dense>

template <typename T>
struct PoseBase {
    using Vector2 = Eigen::Matrix<T, 2, 1>;
    using Vector3 = Eigen::Matrix<T, 3, 1>;
    using Matrix2 = Eigen::Matrix<T, 2, 2>;
    using Matrix3 = Eigen::Matrix<T, 3, 3>;
    T x;
    T y;
    T theta;
    PoseBase() : x(0), y(0), theta(0) {}
    // loop: using while loop for normalizing angle to (-pi, pi]
    PoseBase(T x, T y, T theta, bool loop = false): x(x), y(y) {
        if (loop == false)
            this->theta = goodAngle(theta);
        else
            this->theta = loopNormalize(theta);
    }
    PoseBase(const PoseBase& p): x(p.x), y(p.y), theta(p.theta) {}
    PoseBase(Vector3 p, bool loop = false) : x(p(0)), y(p(1)) {
        if (loop == false)
            this->theta = goodAngle(p(2));
        else
            this->theta = loopNormalize(p(2));
    }
    PoseBase(Vector2 p, T theta):
        x(p(0)), y(p(1)), theta(theta) {}
    PoseBase(const Matrix2& R, const Vector2& t):
        x(t.x()), y(t.y()), theta(atan2(R(1, 0), R(0, 0))) {}
    PoseBase(const Matrix3& M):
        x(M(0, 2)), y(M(1, 2)),
        theta(atan2(M(1, 0), M(0, 0))) {}
    Vector3 eigen() const {
        return Vector3(x, y, theta);
    }
    Vector3 weighted_eigen(T theta_weight) const {
        return Vector3(x, y, theta * theta_weight);
    }
    T norm2d() const {
        return std::sqrt(x * x + y * y);
    }
    void operator+=(Vector3 p) {
        x += p(0);
        y += p(1);
        theta = goodAngle(theta + p(2));
    }
    void operator-=(Vector3 p) {
        x -= p(0);
        y -= p(1);
        theta = goodAngle(theta - p(2));
    }
    PoseBase operator-(const PoseBase& p) const {
        return PoseBase(x - p.x, y - p.y, goodAngle(theta - p.theta));
    }
    PoseBase operator*(const PoseBase& p) const {
        return PoseBase(rotation() * p.translation() + translation(), goodAngle(theta + p.theta));
    }
    Vector2 translation() const {
        return Vector2(x, y);
    }
    Matrix2 rotation() const {
        Matrix2 rotate_mat;
        const T cosa = cos(theta), sina = sin(theta);
        rotate_mat << cosa, -sina, sina, cosa;
        return rotate_mat;
    }
    PoseBase inverse() const {
        T cos_theta = cos(theta);
        T sin_theta = sin(theta);
        T trans_x = x * cos_theta + y * sin_theta;
        T trans_y = x * sin_theta - y * cos_theta;
        return PoseBase(-trans_x, trans_y, -theta);
    }
    Matrix3 transform() const {
        const T cosa = cos(theta), sina = sin(theta);
        const Matrix3 res = {cosa, -sina, x, sina, cosa, y, 0, 0, 1};
        return res;
    }
    bool isnan() const {
        return (std::isnan(x) || std::isnan(y) || std::isnan(theta));
    }
    static PoseBase Identity() {
        return PoseBase(0, 0, 0);
    }
    static T goodAngle(T angle) {
        if (angle > M_PI)
            angle -= 2 * M_PI;
        if (angle < -M_PI)
            angle += 2 * M_PI;
        return angle;
    }
    static T loopNormalize(T angle) {
        while (angle > M_PI)
            angle -= 2 * M_PI;
        while (angle < -M_PI)
            angle += 2 * M_PI;
        return angle;
    }
};

typedef PoseBase<double> Pose;


#ifndef RESOURCE_HPP
#define RESOURCE_HPP

#include "generated_config.hpp"

#include <boost/container/small_vector.hpp>
#include <type_traits>
#include <utility> // for pair
#include <vector>  // for allocator, vector

// one term: coefficient, exponent
typedef std::pair<double, double> poly_term;

// list of terms
// TODO make number of optimized poly-terms a compile-time variable?
typedef std::vector<poly_term> polynomial;

double apply_polynomial(const polynomial & poly, double x);
// TODO add moving variant for efficiency
polynomial add_poly(const polynomial & lhs, const polynomial & rhs);

// Forwards
class Instance;

class Availability {
public:
	Availability(double start_amount);
	Availability(const Availability & other) = default; // copy constructor

	void set(std::vector<std::pair<unsigned int, double>> && new_points);
	double get_at(unsigned int point) const noexcept;

	double get_flat_available() const;

	std::vector<std::pair<unsigned int, double>>::const_iterator begin() const;
	std::vector<std::pair<unsigned int, double>>::const_iterator end() const;

private:
	/* Each pair in the points vector is one step of a stepwise function
	 * indicating the availability of a resource. The first member of every point
	 * indicates from which time step on the amount is available, the second
	 * member indicates the amount. The first point (i.e., the first member of the
	 * first point) must be 0. The points must be sorted by ascending time steps.
	 */
	std::vector<std::pair<unsigned int, double>> points;
};

class FlexCost {
public:
	FlexCost(polynomial base);

	void
	set_flexible(std::vector<std::pair<unsigned int, polynomial>> && new_points);

	const polynomial & get_at(unsigned int point) const noexcept;
	const polynomial & get_base() const noexcept;

	bool is_flat() const;

	std::vector<std::pair<unsigned int, polynomial>>::const_iterator
	begin() const;
	std::vector<std::pair<unsigned int, polynomial>>::const_iterator end() const;

private:
	polynomial base;
	// Same idea as for Availability
	std::vector<std::pair<unsigned int, polynomial>> points;
};

class Resource {
public:
	explicit Resource(unsigned int id);

	void set_availability(Availability && availability);
	void set_overshoot_costs(FlexCost && cost);
	void set_investment_costs(polynomial costs);

	const Availability & get_availability() const;

	const FlexCost & get_flex_overshoot() const;
	const polynomial & get_overshoot_costs(unsigned int pos) const;

	// This only works if the instance has flat costs!
	const polynomial & get_overshoot_costs() const;
	bool is_overshoot_flat() const;

	const polynomial & get_investment_costs() const;

	unsigned int get_rid();
	void set_id(unsigned int id);

	bool
	operator==(const Resource & other) const
	{
		return other.rid == this->rid;
		// TODO in debug mode, compare everything!
	}

	// deepcopy
	Resource clone() const;

private:
	unsigned int rid;
	Availability availability;
	polynomial investment_costs;
	FlexCost overshoot_costs;
};

class ResVec
    : public boost::container::small_vector<double, OPTIMAL_RESOURCE_COUNT> {
public:
	using boost::container::small_vector<double,
	                                     OPTIMAL_RESOURCE_COUNT>::small_vector;
};

class Resources {
public:
	Resources();
	Resources(double usage);
	Resources(const Instance * instance, const ResVec & usage);
	Resources(const Instance * instance, const std::vector<double> & usage);
	Resources(const Instance * instance, ResVec && usage);
	Resources(const Instance * instance);

	const ResVec & getUsage() const;
	ResVec & getUsage();

	Resources operator+(const Resources & other) const;
	Resources operator-(const Resources & other) const;
	Resources operator*(const Resources & other) const;
	Resources operator/(const Resources & other) const;
	void operator+=(const Resources & other);
	void operator-=(const Resources & other);
	void operator*=(const Resources & other);
	void operator/=(const Resources & other);

	bool operator<(const Resources & other) const;
	bool operator>(const Resources & other) const;
	bool operator<=(const Resources & other) const;
	bool operator>=(const Resources & other) const;
	bool operator!=(const Resources & other) const;
	bool operator==(const Resources & other) const;

	template <typename T>
	inline friend std::enable_if_t<std::is_integral_v<T>, Resources>
	operator*(const T & scalar, const Resources & resource);

private:
	const Instance * instance;
	mutable bool cached;
	mutable double cache;

	ResVec usage;

	double getCosts() const;
};

template <typename T>
inline std::enable_if_t<std::is_integral_v<T>, Resources>
operator*(const T & scalar, const Resources & resource)
{
	Resources res(resource.instance, resource.usage);
	for (size_t i = 0; i < resource.usage.size(); i++) {
		res.usage[i] *= scalar;
	}
	return res;
}

template <typename T, typename = std::enable_if_t<std::is_integral_v<T>>>
Resources
operator*(const Resources & resource, const T & scalar)
{
	return scalar * resource;
}

#endif


#include <vector>

#include <glm/glm.hpp>

#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "Types.h"
#include "Dynamic.h"

namespace BalloonFEM
{
    void Engine::computeAirForces(ObjState &state, Vvec3 &f_sum)
    {
        /* add air pressure force */
        for (size_t i = 0; i < m_tetra->holes.size(); i++)
        {
            double p = m_air_model->pressure(state.hole_volume[i]);
            Hole &h = m_tetra->holes[i];
            std::vector<size_t>::iterator j;

            for (j = h.vertices.begin(); j != h.vertices.end(); j++)
            {
                f_sum[*j] += p * state.volume_gradient[*j];
            }
        }
    }

    SpMat Engine::computeAirDiffMat(ObjState &state)
    {
	    printf("building air pressure differential matrix \n");
		/* project from constrained freedom state to world space */
		Vvec3 &pos = state.world_space_pos;

		/* compute air pressure force differential matrix */
		SpMat E = state.volumeGradientDiffMat();

		std::vector<T> pressure;
		pressure.reserve(pos.size());
		for (size_t i = 0; i < m_tetra->holes.size(); i++)
		{
			double p = m_air_model->pressure(state.hole_volume[i]);
			Hole &h = m_tetra->holes[i];
			std::vector<size_t>::iterator j;

			//double dV = 0;
			for (j = h.vertices.begin(); j != h.vertices.end(); j++)
			{
				/* p(V) * dG */
				pressure.push_back( T(3 * *j    , 3 * *j    , p) );
				pressure.push_back( T(3 * *j + 1, 3 * *j + 1, p) );
				pressure.push_back( T(3 * *j + 2, 3 * *j + 2, p) );
			}
		}
		SpMat P(3 * pos.size(), 3 * pos.size());
		P.setFromTriplets(pressure.begin(), pressure.end());
		E = E * P;

        return E;
    }
}


// Testing Jabc for random states
#include "../include/Jabc.hpp"
#include <armadillo>
#include <iostream>
#include <stdlib.h>
#include <math.h>

using namespace std;
using namespace arma;

int main(int argc, char *argv[])
{
  int a = atoi(argv[1]);
  int b = atoi(argv[2]);
  int c = atoi(argv[3]);
  int n_q = atoi(argv[4]);

  cx_mat psi_r, psi_i, psi;
  psi_r.randn(1<<n_q, 1);
  psi_i.randn(1<<n_q, 1);
  psi = psi_r + cx_double(0.0, 1.0) * psi_i;

  psi /= norm(psi);

  cout << Jabc(a,b,c,psi)<< endl;
}


#include "IKinematics.hpp"

#include <cmath>

#include <Eigen/Eigen>

using namespace Eigen;

static float crop(float angle)
{
   while (angle <= -M_PI) angle += 2*M_PI;
   while (angle >   M_PI) angle -= 2*M_PI;

   return angle;
}

namespace IKinematics
{

/**
 * (Very) luckily for us, the nao's leg joint chain can be separated out
 * into reasonably simple independent chains. For simplicity, we assume that
 * the chain link from the ankle to the ground in negligible.
 *
 * Define the following:
 *   x:      axis extending forwards from the robot's torso (sagittal plane)
 *   y:      axis extending rightwards of the robot's torso (coronal plane )
 *   z:      axis extending upwards of the robot's torso
 *   rot:    rotation of the foot relative to the x axis, 0 is forward
 *
 * When rot is 0, the x and y chains are easily separable as only the
 * thigh roll can affects the y axis. Therefore
 *
 *    thighRoll = atan2(z, y)
 *
 * Two joints affect the x axis, namely thighPitch and kneePitch. Note
 * however that the chain has been rotated however, and the new target z'
 * is now the hypotenuse of the triangle created by target z y.
 *
 *    z' = cos(thighPitch + kneePitch)thighL + cos(kneePitch)shinL
 *    x  = sin(thighPitch + kneePitch)thighL + sin(kneePitch)shinL
 *
 * It's pretty obvious that there is a periodic solution to the above
 * equations. However, it is a hell of a lot easier just to use the
 * law of cosines to get the angles.
 *
 *  |\        -     |  We know `thigh', `shin' and the third length of
 *  |A\ thigh |     |  the triangle is simply hypot(z', x). Villa, solve
 *   | \      | z'  |  for B and you're done.
 *   | B\     |     |
 *    |C/     |     |  Note that thigh pitch is not the same as A. In
 *    |/ shin -     |  actual fact, it is a lot easier to calculate
 *                  |  thigh pitch by simply substituting the value of
 *  |-|             |  knee pitch into the above equations.
 *    x             |
 *
 * Relating the above logic back to the original equation, we note that
 * kneePos = pi + B. A rather nice result is that B is always positive
 * assuming we take the positive arc-cosine. We do not want to consider
 * the negative case because this would imply the knee is bending backwards.
 *
 * For rotation, note that the only way to rotate the foot in the lateral
 * plane is to alter the hip pitch (the joint is diagonal). Rotating the
 * chain causes the x and y chains to now depend on each other, as the pitch
 * and roll of each joint now affect both x and y. Easy solution, transform
 * the target using the rotation matrix of the hip. Now the x and y axes are
 * independent again.
 *
 * Now to deal with the negligible foot joint, it turns out its actually pretty
 * easy to add to the chain.
 *
 *    let a = thighPitch
 *        b = kneePitch
 *        c = anklePitch
 *
 *    0  = a + b + c
 *    z' = cos(a + b + c)footL + cos(a + b)shinL + cos(a)thighL
 *    x  = sin(a + b + c)footL + sin(a + b)shinL + sin(a)thighL
 *
 * cos(a + b + c) is a constant, so we can simply remove it both sides of
 * the equation and solve using the same triangle as above. Too easy. In
 * the same manner, we can add the ankle roll to the chain to reach our
 * y-target.
 */

NaoLegChain NaoSolve(const NaoFootTarget &target, float left)
{
   const float thighL = 0.10000;
   const float shinL  = 0.10290;
   const float footL  = 0.04519;

   NaoLegChain r;
   float x, y, z;

   /* For now assume hip yaw is exactly equal to foot yaw.
    * Fuck my life http://en.wikipedia.org/wiki/Universal_joint
    * Well, its actually reasonably linear:
    * http://www.wolframalpha.com/input/?i=plot+atan(tan(x)/cos(pi/4)))
    *
    * This is an expensive operation so dont do it unless we actually need to.
    */
   r.hip = target.yaw;
   if (target.yaw == 0) {
      x = target.x;
      y = target.y;
      z = target.z;
   } else {
      Vector3f v(target.x, target.y, target.z);
      AngleAxis<float> aa(-target.yaw, Vector3f(0, sqrtf(2)/2, sqrtf(2)/2));
      v = aa * v;
      x = v.x(); y = v.y(); z = v.z();
   }

   /* Subtract foot length from target. This can be seen as the vector
    *    target' = target - Rot_roll * Rot_pitch * foot
    *
    * The fact that pitch is applied before roll is due to the robot's
    * physical design.
    */

   x -= footL * sinf(target.pitch);
   y -= footL * cosf(target.pitch) * sinf(target.roll);
   z += footL * cosf(target.pitch) * cosf(target.roll);

   r.thighRoll = atan2f(y, -z);

   /* Solve for target.x and target.z */
   const float z_prime = z / cosf(r.thighRoll);
   const float h = hypotf(z_prime, x);

   /* A B and C are as above comment */
   const float A = acosf((shinL*shinL - h*h - thighL*thighL) / (-2*thighL*h));
   const float B = acosf((h*h - thighL*thighL - shinL*shinL) / (-2*thighL*shinL));
   r.thighPitch = A + asin(x/h) + 3*M_PI_2;
   r.kneePitch  = B + M_PI;

   /* Calculate the ankle pitch and roll */
   r.anklePitch = target.pitch - r.kneePitch - r.thighPitch;
   r.ankleRoll  = target.roll - r.thighRoll;

   /* Currently all angles are wound counter clockwise from the normal.
    * Joints on the actual robot each have their own normals. Basically,
    * when all joint values are set to zero the robot is standing upright.
    */
   r.thighPitch = 3*M_PI_2 - r.thighPitch;
   r.kneePitch  =          - r.kneePitch;
   r.anklePitch = 1*M_PI_2 - r.anklePitch;

   /* Finally make sure all angles are bound by (-pi, pi] */
   r.hip        = crop(r.hip       );
   r.thighRoll  = crop(r.thighRoll );
   r.thighPitch = crop(r.thighPitch);
   r.kneePitch  = crop(r.kneePitch );
   r.ankleRoll  = crop(r.ankleRoll );
   r.anklePitch = crop(r.anklePitch);

   return r;
}

std::ostream &operator <<(std::ostream &out, const NaoLegChain &chain)
{
   out << "chain.hip        = " << chain.hip        << std::endl;
   out << "chain.thighRoll  = " << chain.thighRoll  << std::endl;
   out << "chain.thighPitch = " << chain.thighPitch << std::endl;
   out << "chain.kneePitch  = " << chain.kneePitch  << std::endl;
   out << "chain.ankleRoll  = " << chain.ankleRoll  << std::endl;
   out << "chain.anklePitch = " << chain.anklePitch << std::endl;

   return out;
}

std::ostream &operator <<(std::ostream &out, const NaoFootTarget &target)
{
   out << "x     =" << target.x     << std::endl;
   out << "y     =" << target.y     << std::endl;
   out << "z     =" << target.z     << std::endl;
   out << "pitch =" << target.pitch << std::endl;
   out << "roll  =" << target.roll  << std::endl;
   out << "yaw   =" << target.yaw   << std::endl;

   return out;
}

} /* namespace IKinematics */


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/oneotwoeps.hpp>
#include <boost/simd/function/scalar/log.hpp>
#include <boost/simd/function/scalar/log1p.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/simd/function/scalar/sqrt.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( acosh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      A0 t = minusone(a0);
      if(BOOST_LIKELY(t <= Oneotwoeps<A0>()))
        return log1p(t+bs::sqrt(t+t+sqr(t)));
      else
        return log(t)+Log_2<A0>();
    }
  };
} } }

#endif


#include <Eigen/Geometry>
#include <iostream>

#include "align/rigid_pipe.h"
#include "face/model.h"
#include "util/eigen_pcl.h"

using namespace std;
using namespace telef::feature;
using namespace telef::types;
using namespace telef::face;

namespace telef::align {

PCARigidFittingPipe::PCARigidFittingPipe() :
    m_prev_scale(0.0f)
{}

boost::shared_ptr<PCANonRigidAlignmentSuite> PCARigidFittingPipe::_processData(
    boost::shared_ptr<PCANonRigidAlignmentSuite> in) {
  std::vector<int> pca_lmks = in->pca_model->getLandmarks();
  auto in_lmks = in->fittingSuite->landmark3d;

  Eigen::VectorXf ref =
    in->pca_model->genPosition(in->shapeCoeff, in->expressionCoeff);
  Eigen::Matrix3Xf mesh_pts_t =
      Eigen::Map<Eigen::Matrix3Xf>(ref.data(), 3, ref.size() / 3);
  std::vector<int> selection = {
                                0, 1, 2, 3,
                                13, 14, 15, 16,
                                27, 28, 29, 30,
                                33, 36, 39, 42, 45};
  Eigen::MatrixXf mesh_lmk_pts(selection.size(), 3);

  for (int i = 0; i < selection.size(); i++) {
    mesh_lmk_pts.row(i) = mesh_pts_t.col(pca_lmks[selection[i]]);
  }

  Eigen::MatrixXf lmk_pts(selection.size(), 3);
  for (int i = 0; i < selection.size(); i++) {
    lmk_pts(i, 0) = in_lmks->points[selection[i]].x;
    lmk_pts(i, 1) = in_lmks->points[selection[i]].y;
    lmk_pts(i, 2) = in_lmks->points[selection[i]].z;
  }

  Eigen::MatrixXf transformation;
  if(m_prev_scale == 0.0f)
    {
      transformation = Eigen::umeyama(mesh_lmk_pts.transpose(), lmk_pts.transpose());
      m_prev_scale = transformation.block(0,0,3,0).norm();
    }
  else
    {
      transformation = Eigen::umeyama(
          m_prev_scale*mesh_lmk_pts.transpose(), lmk_pts.transpose(), false);
      transformation.block(0,0,3,3) *= m_prev_scale;
    }

  in->transformation = transformation;
  return in;
}
} // namespace telef::align