//---------------------------------------------------------------------------//
//!
//! \file   tstPolynomialDistribution.cpp
//! \author Alex Robinson
//! \brief  Polynomial distribution unit tests
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

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

// FRENSIE Includes
#include "Utility_PolynomialDistribution.hpp"
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_ElectronVoltUnit.hpp"
#include "Utility_UnitTestHarnessWithMain.hpp"
#include "ArchiveTestHelpers.hpp"

//---------------------------------------------------------------------------//
// Testing Types
//---------------------------------------------------------------------------//

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

typedef TestArchiveHelper::TestArchives TestArchives;

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

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

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

std::shared_ptr<Utility::UnitAwareUnivariateDistribution<MegaElectronVolt,si::amount> >
  unit_aware_distribution;

//---------------------------------------------------------------------------//
// Tests.
//---------------------------------------------------------------------------//
// Check that the distribution can be evaluated
FRENSIE_UNIT_TEST( PolynomialDistribution, evaluate )
{
  FRENSIE_CHECK_EQUAL( distribution->evaluate( -1.0 ), 0.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluate( 0.0 ), 1.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluate( 0.5 ), 2.75 );
  FRENSIE_CHECK_EQUAL( distribution->evaluate( 1.0 ), 6.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluate( 2.0 ), 0.0 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be evaluated
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, evaluate )
{
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( -1.0*MeV ),
		       0.0*si::mole );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( 0.0*MeV ),
		       1.0*si::mole );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( 0.5*MeV ),
		       2.75*si::mole );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( 1.0*MeV ),
		       6.0*si::mole );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluate( 2.0*MeV ),
		       0.0*si::mole );
}

//---------------------------------------------------------------------------//
// Check that the PDF can be evaluated
FRENSIE_UNIT_TEST( PolynomialDistribution, evaluatePDF )
{
  FRENSIE_CHECK_EQUAL( distribution->evaluatePDF( -1.0 ), 0.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluatePDF( 0.0 ), 1.0/3.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluatePDF( 0.5 ), 2.75/3.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluatePDF( 1.0 ), 2.0 );
  FRENSIE_CHECK_EQUAL( distribution->evaluatePDF( 2.0 ), 0.0 );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware PDF can be evaluated
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, evaluatePDF )
{
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluatePDF( -1.0*MeV ),
		       0.0/MeV );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluatePDF( 0.0*MeV ),
		       (1.0/3.0)/MeV );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluatePDF( 0.5*MeV ),
		       (2.75/3.0)/MeV );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluatePDF( 1.0*MeV ),
		       2.0/MeV );
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->evaluatePDF( 2.0*MeV ),
		       0.0/MeV );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
FRENSIE_UNIT_TEST( PolynomialDistribution, sample )
{
  std::vector<double> fake_stream( 6 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.0;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );
  double sample = distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.0 );

  sample = distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.5 );

  fake_stream[0] = 0.5;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.5;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );
  sample = distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.0 );

  sample = distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, sqrt(0.5) );

  fake_stream[0] = 0.7;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.7;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );
  sample = distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.0 );

  sample = distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, pow( 0.5, 1.0/3.0 ) );
}

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

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );
  quantity<MegaElectronVolt> sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.0*MeV );

  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.5*MeV );

  fake_stream[0] = 0.5;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.5;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );
  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.0*MeV );

  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, sqrt(0.5)*MeV );

  fake_stream[0] = 0.7;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.7;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );
  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, 0.0*MeV );

  sample = unit_aware_distribution->sample();
  FRENSIE_CHECK_EQUAL( sample, pow( 0.5, (1.0/3.0) )*MeV );
}

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
FRENSIE_UNIT_TEST( PolynomialDistribution, sampleAndRecordTrials )
{
  std::vector<double> fake_stream( 6 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.0;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  Utility::DistributionTraits::Counter trials = 0;

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

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

  fake_stream[0] = 0.5;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.5;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  trials = 0;

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

  sample = distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, sqrt(0.5) );
  FRENSIE_CHECK_EQUAL( 2.0/trials, 1.0 );

  fake_stream[0] = 0.7;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.7;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  trials = 0;

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

  sample = distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, pow( 0.5, 1.0/3.0 ) );
  FRENSIE_CHECK_EQUAL( 2.0/trials, 1.0 );
}

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

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  Utility::DistributionTraits::Counter trials = 0;

  quantity<MegaElectronVolt> sample =
    unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, 0.0*MeV );
  FRENSIE_CHECK_EQUAL( 1.0/trials, 1.0 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, 0.5*MeV );
  FRENSIE_CHECK_EQUAL( 2.0/trials, 1.0 );

  fake_stream[0] = 0.5;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.5;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  trials = 0;

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, 0.0*MeV );
  FRENSIE_CHECK_EQUAL( 1.0/trials, 1.0 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, sqrt(0.5)*MeV );
  FRENSIE_CHECK_EQUAL( 2.0/trials, 1.0 );

  fake_stream[0] = 0.7;
  fake_stream[1] = 0.0;
  fake_stream[2] = 0.7;
  fake_stream[3] = 0.5;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  trials = 0;

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, 0.0*MeV );
  FRENSIE_CHECK_EQUAL( 1.0/trials, 1.0 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  FRENSIE_CHECK_EQUAL( sample, pow( 0.5, 1.0/3.0 )*MeV );
  FRENSIE_CHECK_EQUAL( 2.0/trials, 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the upper bound of the distribution independent variable can be
// returned
FRENSIE_UNIT_TEST( PolynomialDistribution, getUpperBoundOfIndepVar )
{
  FRENSIE_CHECK_EQUAL( distribution->getUpperBoundOfIndepVar(), 1.0 );
}

//---------------------------------------------------------------------------//
// Check that the upper bound of the unit-aware distribution independent
// variable can be returned
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, getUpperBoundOfIndepVar )
{
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->getUpperBoundOfIndepVar(),
		       1.0*MeV );
}

//---------------------------------------------------------------------------//
// Check that the lower bound of the unit-aware distribution independent
// variable can be returned
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, getLowerBoundOfIndepVar )
{
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->getLowerBoundOfIndepVar(),
		       0.0*MeV );
}

//---------------------------------------------------------------------------//
// Check that the distribution type can be returned
FRENSIE_UNIT_TEST( PolynomialDistribution, getDistributionType )
{
  FRENSIE_CHECK_EQUAL( distribution->getDistributionType(),
		       Utility::POLYNOMIAL_DISTRIBUTION );
}

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution type can be returned
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, getDistributionType )
{
  FRENSIE_CHECK_EQUAL( unit_aware_distribution->getDistributionType(),
		       Utility::POLYNOMIAL_DISTRIBUTION );
}

//---------------------------------------------------------------------------//
// Check if the distribution is tabular
FRENSIE_UNIT_TEST( PolynomialDistribution, isTabular )
{
  FRENSIE_CHECK( !distribution->isTabular() );
}

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is tabular
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, isTabular )
{
  FRENSIE_CHECK( !unit_aware_distribution->isTabular() );
}

//---------------------------------------------------------------------------//
// Check if the distribution is continuous
FRENSIE_UNIT_TEST( PolynomialDistribution, isContinuous )
{
  FRENSIE_CHECK( distribution->isContinuous() );
}

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is continuous
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, isContinuous )
{
  FRENSIE_CHECK( unit_aware_distribution->isContinuous() );
}

//---------------------------------------------------------------------------//
// Check if the distribution is compatible with the interpolation type
FRENSIE_UNIT_TEST( PolynomialDistribution, isCompatibleWithInterpType )
{
  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LinLog>() );
  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( distribution->isCompatibleWithInterpType<Utility::LogLog>() );

  // Create another distribution that is compatible with all interpolation
  // types
  std::vector<double> coeffs( 4 );
  coeffs[0] = 1.0;
  coeffs[1] = 2.0;
  coeffs[2] = 3.0;
  coeffs[3] = 0.0;

  std::shared_ptr<Utility::PolynomialDistribution>
    test_dist( new Utility::PolynomialDistribution( coeffs, 1.0, 2.0 ) );

  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLog>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LogLog>() );

  // Create another distribution that is only compatible with lin-lin interp
  coeffs[0] = 0.0;

  test_dist.reset( new Utility::PolynomialDistribution( coeffs, 0.0, 1.0 ) );

  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLog>() );
  FRENSIE_CHECK( !test_dist->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( !test_dist->isCompatibleWithInterpType<Utility::LogLog>() );
}

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is compatible with the interp type
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution,
                   isCompatibleWithInterpType )
{
  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LinLog>() );
  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( unit_aware_distribution->isCompatibleWithInterpType<Utility::LogLog>() );

  // Create another distribution that is compatible with all interpolation
  // types
  std::vector<double> coeffs( 4 );
  coeffs[0] = 1.0;
  coeffs[1] = 2.0;
  coeffs[2] = 3.0;
  coeffs[3] = 0.0;

  std::shared_ptr<Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount> >
    test_dist( new Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>( coeffs, 1.0*MeV, 2.0*MeV ) );

  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLog>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LogLog>() );

  // Create another distribution that is only compatible with lin-lin interp
  coeffs[0] = 0.0;

  test_dist.reset( new Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>( coeffs, 0.0*MeV, 1.0*MeV ) );

  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLin>() );
  FRENSIE_CHECK( test_dist->isCompatibleWithInterpType<Utility::LinLog>() );
  FRENSIE_CHECK( !test_dist->isCompatibleWithInterpType<Utility::LogLin>() );
  FRENSIE_CHECK( !test_dist->isCompatibleWithInterpType<Utility::LogLog>() );
}

//---------------------------------------------------------------------------//
// Check that a distribution can be placed in a stream
FRENSIE_UNIT_TEST( PolynomialDistribution, ostream_operator )
{
  std::ostringstream oss;

  oss << Utility::PolynomialDistribution();

  Utility::VariantMap dist_data =
    Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({1.0}),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toDouble(), 0.0, SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toDouble(), 1.0, SHOW_LHS );

  oss.str( "" );
  oss.clear();

  oss << Utility::PolynomialDistribution( {0.0, 1.0, 0.0, 1.0} );

  dist_data = Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({0.0, 1.0, 0.0, 1.0}) );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toDouble(), 0.0 );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toDouble(), 1.0 );

  oss.str( "" );
  oss.clear();

  oss << Utility::PolynomialDistribution( {0.0, 1.0, 0.0, 1.0}, 1.0, 2.0 );

  dist_data = Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({0.0, 1.0, 0.0, 1.0}) );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toDouble(), 1.0 );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toDouble(), 2.0 );

  oss.str( "" );
  oss.clear();

  oss << *distribution;

  dist_data = Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(), "void", SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({1.0, 2.0, 3.0, 0.0}) );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toDouble(), 0.0 );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toDouble(), 1.0 );
}

//---------------------------------------------------------------------------//
// Check that a unit-aware distribution can be placed in a stream
FRENSIE_UNIT_TEST( UnitAwarePolynomialDistribution, ostream_operator )
{
  std::ostringstream oss;

  oss << Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>();

  Utility::VariantMap dist_data =
    Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(),
                       Utility::UnitTraits<MegaElectronVolt>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(),
                       Utility::UnitTraits<si::amount>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({1.0}),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toType<quantity<MegaElectronVolt> >(),
                       0.0*MeV,
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toType<quantity<MegaElectronVolt> >(),
                       1.0*MeV,
                       SHOW_LHS );

  oss.str( "" );
  oss.clear();

  oss << Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>( {0.0, 1.0, 0.0, 1.0} );

  dist_data = Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(),
                       Utility::UnitTraits<MegaElectronVolt>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(),
                       Utility::UnitTraits<si::amount>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({0.0, 1.0, 0.0, 1.0}),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toType<quantity<MegaElectronVolt> >(),
                       0.0*MeV,
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toType<quantity<MegaElectronVolt> >(),
                       1.0*MeV,
                       SHOW_LHS );

  oss.str( "" );
  oss.clear();

  oss << Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>( {0.0, 1.0, 0.0, 1.0}, 1.0*MeV, 2.0*MeV );

  dist_data = Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(),
                       Utility::UnitTraits<MegaElectronVolt>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(),
                       Utility::UnitTraits<si::amount>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({0.0, 1.0, 0.0, 1.0}),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toType<quantity<MegaElectronVolt> >(),
                       1.0*MeV,
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toType<quantity<MegaElectronVolt> >(),
                       2.0*MeV,
                       SHOW_LHS );

  oss.str( "" );
  oss.clear();

  oss << *unit_aware_distribution;

  dist_data = Utility::fromString<Utility::VariantMap>( oss.str() );

  FRENSIE_CHECK_EQUAL( dist_data["type"].toString(),
                       "Polynomial Distribution" );
  FRENSIE_CHECK_EQUAL( dist_data["independent unit"].toString(),
                       Utility::UnitTraits<MegaElectronVolt>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["dependent unit"].toString(),
                       Utility::UnitTraits<si::amount>::name(),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["coefficients"].toType<std::vector<double> >(),
                       std::vector<double>({1.0, 2.0, 3.0, 0.0}),
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["lower bound"].toType<quantity<MegaElectronVolt> >(),
                       0.0*MeV,
                       SHOW_LHS );
  FRENSIE_CHECK_EQUAL( dist_data["upper bound"].toType<quantity<MegaElectronVolt> >(),
                       1.0*MeV,
                       SHOW_LHS );
}

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

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

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

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

    createOArchive( archive_base_name, archive_ostream, oarchive );

    Utility::PolynomialDistribution dist_a( {3.0, 2.0, 1.0}, 1.0, 2.0 );

    FRENSIE_REQUIRE_NO_THROW(
                             (*oarchive) << BOOST_SERIALIZATION_NVP( dist_a ) );
    FRENSIE_REQUIRE_NO_THROW( (*oarchive) << boost::serialization::make_nvp( "dist_b", distribution ) );
  }

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

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

  createIArchive( archive_istream, iarchive );

  Utility::PolynomialDistribution dist_a;

  FRENSIE_REQUIRE_NO_THROW(
                           (*iarchive) >> BOOST_SERIALIZATION_NVP(dist_a) );
  FRENSIE_CHECK_EQUAL( dist_a, Utility::PolynomialDistribution( {3.0, 2.0, 1.0}, 1.0, 2.0 ) );

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

  FRENSIE_REQUIRE_NO_THROW(
                           (*iarchive) >> BOOST_SERIALIZATION_NVP(dist_b) );
  FRENSIE_CHECK_EQUAL( *dynamic_cast<Utility::PolynomialDistribution*>(dist_b.get()),
                       *dynamic_cast<Utility::PolynomialDistribution*>(distribution.get()) );
}

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

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

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

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

    createOArchive( archive_base_name, archive_ostream, oarchive );

    Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount> dist_a( {3.0, 2.0, 1.0}, 1.0*MeV, 2.0*MeV );

    FRENSIE_REQUIRE_NO_THROW(
                             (*oarchive) << BOOST_SERIALIZATION_NVP( dist_a ) );
    FRENSIE_REQUIRE_NO_THROW( (*oarchive) << boost::serialization::make_nvp( "dist_b", unit_aware_distribution ) );
  }

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

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

  createIArchive( archive_istream, iarchive );

  Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount> dist_a;

  FRENSIE_REQUIRE_NO_THROW(
                           (*iarchive) >> BOOST_SERIALIZATION_NVP(dist_a) );
  FRENSIE_CHECK_EQUAL( dist_a, (Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>( {3.0, 2.0, 1.0}, 1.0*MeV, 2.0*MeV )) );

  std::shared_ptr<Utility::UnitAwareUnivariateDistribution<MegaElectronVolt,si::amount> > dist_b;

  FRENSIE_REQUIRE_NO_THROW(
                           (*iarchive) >> BOOST_SERIALIZATION_NVP(dist_b) );
  FRENSIE_CHECK_EQUAL( (*dynamic_cast<Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>*>(dist_b.get())),
                       (*dynamic_cast<Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>*>(unit_aware_distribution.get())) );
}

//---------------------------------------------------------------------------//
// Check that a unit-aware distribution can be scaled
FRENSIE_UNIT_TEST_TEMPLATE_EXPAND( UnitAwarePolynomialDistribution,
				   explicit_conversion,
                                   TestUnitTypeQuads )
{
  FETCH_TEMPLATE_PARAM( 0, RawIndepUnitA );
  FETCH_TEMPLATE_PARAM( 1, RawDepUnitA );
  FETCH_TEMPLATE_PARAM( 2, RawIndepUnitB );
  FETCH_TEMPLATE_PARAM( 3, RawDepUnitB );

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

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

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

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

  // Copy from a unitless distribution to distribution type A (static method)
  Utility::UnitAwarePolynomialDistribution<IndepUnitA,DepUnitA>
    unit_aware_dist_a_copy = Utility::UnitAwarePolynomialDistribution<IndepUnitA,DepUnitA>::fromUnitlessDistribution( *dynamic_cast<Utility::PolynomialDistribution*>( distribution.get() ) );

  // Copy from distribution type A to distribution type B (explicit cast)
  Utility::UnitAwarePolynomialDistribution<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( 1/3. );
  DepQuantityA dep_quantity_a =
    Utility::QuantityTraits<DepQuantityA>::initializeQuantity( 1.0 );

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

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

  Utility::setQuantity( indep_quantity_a, 1.0 );
  Utility::setQuantity( inv_indep_quantity_a, 2.0 );
  Utility::setQuantity( dep_quantity_a, 6.0 );

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

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

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

FRENSIE_CUSTOM_UNIT_TEST_INIT()
{
  // Initialize the polynomial distribution
  std::vector<double> coeffs( 4 );
  coeffs[0] = 1.0;
  coeffs[1] = 2.0;
  coeffs[2] = 3.0;
  coeffs[3] = 0.0;

  distribution.reset( new Utility::PolynomialDistribution( coeffs, 0.0, 1.0 ));

  // Initialize the unit-aware polynomial distribution
  unit_aware_distribution.reset(
     new Utility::UnitAwarePolynomialDistribution<MegaElectronVolt,si::amount>(
						  coeffs, 0.0*eV, 1e6*eV ) );

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

FRENSIE_CUSTOM_UNIT_TEST_SETUP_END();

//---------------------------------------------------------------------------//
// end tstPolynomialDistribution.cpp
//---------------------------------------------------------------------------//


#include <fstream>
#include <vector>
#include <iterator>
#include <iostream>
#include <exception>
#include <memory>
// #include <boost/program_options.hpp>

#include <openssl/ec.h>
#include <openssl/err.h>
#include "openssl/conf.h"
#include "openssl/err.h"
#include "openssl/engine.h"
#include "openssl/ssl.h"

#include "TTCN3.hh"
#include "EtsiTs103097Module.hh"
#include "EtsiTs102941MessagesCa.hh"

#include "its/utils.hh"
#include "its/itspki-debug.hh"
#include "its/itspki-etsi.hh"
#include "its/its-asn1-modules.hh"

bool
ItsPkiEtsi::ItsPkiPublicKey::LoadCompressedKey(int in_nid, OCTETSTRING &in_comp_key, int in_comp_mode)
{
	if (!ECKey_DecompressPublicKey(in_nid, in_comp_key, in_comp_mode, x, y))   {
		ERROR_STREAMC << "cannot " << std::endl;
		return false;
	}

	comp_key_mode = INTEGER(in_comp_mode);
	comp_key = in_comp_key;
	nid = in_nid;

	return true;
}

bool
ItsPkiEtsi::ItsPkiPublicKey::LoadVerificationKeyFromCertificate(IEEE1609dot2::CertificateBase &in_cert)
{
	DEBUGC_STREAM_CALLED;

	if (!in_cert.is_present())   {
		ERROR_STREAMC << "invalid argument" << std::endl;
		return false;
	}

	IEEE1609dot2BaseTypes::PublicVerificationKey v_key = in_cert.toBeSigned().verifyKeyIndicator().verificationKey();
	if (!v_key.is_present())   {
		ERROR_STREAMC << "no public verification key" << std::endl;
		return false;
	}

	if (v_key.ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaNistP256)) {
		nid = OpenSSL_txt2nid("prime256v1");
		IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point = v_key.ecdsaNistP256();

		if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__0(), 0))   {
				ERROR_STREAMC << "cannot load Y0 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__1(), 1))   {
				ERROR_STREAMC << "cannot load Y1 compressed key" << std::endl;
				return false;
			}
		}
		else if (v_key.ecdsaNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
			x = ec_point.uncompressedP256().x();
			y = ec_point.uncompressedP256().y();
		}
		else {
			ERROR_STREAMC << "EC point type '" << ec_point.get_selection() << "' not supported" << std::endl;
      			return false;
		}
	}
	else if (v_key.ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaBrainpoolP256r1)) {
		nid = OpenSSL_txt2nid("brainpoolP256r1");
		IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point = v_key.ecdsaBrainpoolP256r1();

		if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__0(), 0))   {
				ERROR_STREAMC << "cannot load Y0 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__1(), 1))   {
				ERROR_STREAMC << "cannot load Y1 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
      			x = ec_point.uncompressedP256().x();
      			y = ec_point.uncompressedP256().y();
		}
		else   {
			ERROR_STREAMC << "EC point type '" << ec_point.get_selection() << "' not supported" << std::endl;
			return false;
		}
	}
	else if (v_key.ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaBrainpoolP384r1)) {
		nid = OpenSSL_txt2nid("brainpoolP384r1");
		IEEE1609dot2BaseTypes::EccP384CurvePoint ec_point = v_key.ecdsaBrainpoolP384r1();

		if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP384CurvePoint::ALT_compressed__y__0)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__0(), 0))   {
				ERROR_STREAMC << "cannot load Y0 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP384CurvePoint::ALT_compressed__y__1)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__1(), 1))   {
				ERROR_STREAMC << "cannot load Y1 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP384CurvePoint::ALT_uncompressedP384)) {
			x = ec_point.uncompressedP384().x();
			y = ec_point.uncompressedP384().y();
		}
		else {
			ERROR_STREAMC << "EC point type '" << ec_point.get_selection() << "' not supported" << std::endl;
			return false;
		}
	}
	else {
		ERROR_STREAMC << "EC curve '" << v_key.get_selection() << "' not supported" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPublicKey::LoadEncryptionKeyFromCertificate(IEEE1609dot2::CertificateBase &in_cert)
{
	DEBUGC_STREAM_CALLED;

	if (!in_cert.is_present())   {
		ERROR_STREAMC << "invalid argument" << std::endl;
		return false;
	}

	IEEE1609dot2BaseTypes::PublicEncryptionKey e_key = in_cert.toBeSigned().encryptionKey();
	if (!e_key.is_present())   {
		ERROR_STREAMC << "no public encryption key" << std::endl;
		return false;
	}

    	if (e_key.publicKey().ischosen(IEEE1609dot2BaseTypes::BasePublicEncryptionKey::ALT_eciesNistP256)) {
		nid = OpenSSL_txt2nid("prime256v1");
		IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point = e_key.publicKey().eciesNistP256();

      		if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__0(), 0))   {
				ERROR_STREAMC << "cannot load Y0 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__1(), 1))   {
				ERROR_STREAMC << "cannot load Y1 compressed key" << std::endl;
				return false;
			}
		}
		else if (e_key.publicKey().eciesNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
			x = ec_point.uncompressedP256().x();
			y = ec_point.uncompressedP256().y();
		}
		else   {
			ERROR_STREAMC << "not supported EC point type '" << ec_point.get_selection() << "'" << std::endl;
			return false;
		}
	}
    	else if (e_key.publicKey().ischosen(IEEE1609dot2BaseTypes::BasePublicEncryptionKey::ALT_eciesBrainpoolP256r1)) {
		nid = OpenSSL_txt2nid("brainpoolP256r1");
		IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point = e_key.publicKey().eciesBrainpoolP256r1();

      		if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__0(), 0))   {
				ERROR_STREAMC << "cannot load Y0 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
			if (!LoadCompressedKey(nid, ec_point.compressed__y__1(), 1))   {
				ERROR_STREAMC << "cannot load Y1 compressed key" << std::endl;
				return false;
			}
		}
		else if (ec_point.ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
			x = ec_point.uncompressedP256().x();
			y = ec_point.uncompressedP256().y();
		}
		else   {
			ERROR_STREAMC << "not supported EC point type '" << ec_point.get_selection() << "'" << std::endl;
			return false;
		}
	}
	else   {
		ERROR_STREAMC << "not supported EC curve '" << e_key.publicKey().get_selection() << "'" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPublicKey::GetXY(OCTETSTRING &_x, OCTETSTRING &_y)
{
	DEBUGC_STREAM_CALLED;

	if (x.is_present() && y.is_present() && x.lengthof() > 0 && y.lengthof() > 0)   {
		_x = x;
		_y = y;
		DEBUGC_STREAM_RETURNS_OK;
		return true;
	}

	ERROR_STREAMC << "failed " << std::endl;
	return false;
}

bool
ItsPkiEtsi::ItsPkiPublicKey::GetCompressed(OCTETSTRING &ret)
{
	DEBUGC_STREAM_CALLED;

	if (!comp_key.is_present() || comp_key.lengthof() <= 0)   {
		ERROR_STREAMC << "failed " << std::endl;
		return false;
	}

	ret = comp_key;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiRecipient::GetCertHash(OCTETSTRING &r_hash)
{
	DEBUGC_STREAM_CALLED;

	if (!cert_hash.is_present() || cert_hash.lengthof() == 0)   {
		ERROR_STREAMC << "failed " << std::endl;
		return false;
	}

	r_hash = cert_hash;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiRecipient::GetHashedID8(OCTETSTRING &ret)
{
	DEBUGC_STREAM_CALLED;

	if (!hashed_id8.is_present() || hashed_id8.lengthof() == 0)
		return false;

	ret = hashed_id8;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiRecipient::ParseCert(IEEE1609dot2::CertificateBase &in_cert)
{
	DEBUGC_STREAM_CALLED;

	if (!in_cert.is_present())   {
		ERROR_STREAMC << "invalid argument" << std::endl;
		return false;
	}

        if (!in_cert.toBeSigned().encryptionKey().is_present())   {
		ERROR_STREAMC << "encryptionKey not present" << std::endl;
		return false;
	}

	cert = in_cert;
        if (!encEtsiTs103097Certificate(cert, cert_blob))    {
                ERROR_STREAMC << "cannot encode EtsiTs103097Certificate\n";
                return false;
        }
	dump_ttcn_object(cert_blob, "reencoded recipient certificate blob: ");

	if (!v_pub_key.LoadVerificationKeyFromCertificate(cert))   {
		ERROR_STREAMC << "cannot load VerificationKey" << std::endl;
		return false;
	}

	if (!e_pub_key.LoadEncryptionKeyFromCertificate(cert))   {
		ERROR_STREAMC << "cannot load EncryptionKey" << std::endl;
		return false;
	}

	int nid = v_pub_key.GetNID();
	if (nid == NID_X9_62_prime256v1 || nid == NID_brainpoolP256r1)   {
		if (!hash_256(cert_blob, cert_hash))   {
			ERROR_STREAMC << "failed to get certificate's hash256" << std::endl;
			return false;
		}
	}
        else if (nid == NID_brainpoolP384r1)   {
		if (!hash_384(cert_blob, cert_hash))   {
			ERROR_STREAMC << "failed to get certificate's hash384" << std::endl;
			return false;
		}
	}
	else   {
		ERROR_STREAMC << "Invalid verification key NID: " << nid << std::endl;
		return false;
	}
	hashed_id8 = OCTETSTRING(8, ((const unsigned char *)cert_hash + cert_hash.lengthof() - 8));
	dump_ttcn_object(hashed_id8, "#### HashedID8: ");

	if (cert.issuer().ischosen(IEEE1609dot2::IssuerIdentifier::ALT_sha256AndDigest)) {
		issuer_id8 = cert.issuer().sha256AndDigest();
	}
	else if (cert.issuer().ischosen(IEEE1609dot2::IssuerIdentifier::ALT_sha384AndDigest)) {
		issuer_id8 = cert.issuer().sha384AndDigest();
	}
	else if (cert.issuer().ischosen(IEEE1609dot2::IssuerIdentifier::ALT_self__)) {
		issuer_id8 = OCTETSTRING(8, ((const unsigned char *)cert_hash + cert_hash.lengthof() - 8));
	}
	else {
		ERROR_STREAMC << "unsupported issuer type '" << cert.issuer().get_selection() << "'" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

ItsPkiEtsi::~ItsPkiEtsi()
{
	OpenSSL_cleanup();
}

bool
ItsPkiEtsi::setRecipient(OCTETSTRING &cert_blob, void *e_prvkey)
{
	DEBUGC_STREAM_CALLED;

	dump_ttcn_object(cert_blob, "recipient certificate blob: ");
	IEEE1609dot2::CertificateBase cert = decEtsiTs103097Certificate(cert_blob);
	dump_ttcn_object(cert, "recipient certificate: ");

	if (!recipient.ParseCert(cert))   {
		ERROR_STREAMC << "cannot parse recipient certificate" << std::endl;
		return false;
	}
	if (e_prvkey)
		recipient.SetPrivateKey(e_prvkey);
	ready = true;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::setDecryptContext(void *prvkey, void *pubkey)
{
	DEBUGC_STREAM_CALLED;

	DEBUGC_STREAM << "PrvKey: " << prvkey << " PubKey: " << pubkey << std::endl;
	recipient.SetPrivateKey(prvkey);
	recipient.SetSenderPublicKey(pubkey);

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::setDecryptContextWithSKey(OCTETSTRING &skey_id)
{
	DEBUGC_STREAM_CALLED;

	if (!enc_key.checkDecryptContext(skey_id))   {
		ERROR_STREAMC << "failed to set decryption context" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::setDecryptContextWithSKey(OCTETSTRING &skey_id, OCTETSTRING &aes_key, OCTETSTRING &tag)
{
	enc_key.setup_skey(aes_key, tag);
	return setDecryptContextWithSKey(skey_id);
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::setup(int in_nid, const char *prvkey_str)
{
	DEBUGC_STREAM_CALLED;
	DEBUGC_STREAM << "nid:" << in_nid << ", prvkey" << std::endl;

	if (!ECKey_KeyComponentsFromString(in_nid, prvkey_str, pubkey.x, pubkey.y, pubkey.comp_key, pubkey.comp_key_mode))   {
		ERROR_STREAMC << "cannot parse private EC key" << std::endl;
		return false;
	}
	pubkey.nid = in_nid;
	prvkey_oct = str2oct(prvkey_str);

	dump_ttcn_object(prvkey_oct, "PrvKey raw: ");
	dump_ttcn_object(pubkey.x, "PrvKey X: ");
	dump_ttcn_object(pubkey.y, "PrvKey Y: ");
	dump_ttcn_object(pubkey.comp_key, "PrvKey CompPubKey: ");
	dump_ttcn_object(pubkey.comp_key_mode, "PrvKey CompPubKeyMode: ");
	DEBUGC_STREAM << "PrvKey NID:" <<  pubkey.nid << std::endl;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::setup(void *in_key, OCTETSTRING &in_enc_skey)
{
	DEBUGC_STREAM_CALLED;

	if (EC_KEY_check_key((EC_KEY *)in_key) == 0)    {
		ERROR_STREAMC << "not valid EC key" << std::endl;
		return false;
	}
	ec_key = in_key;

	enc_skey = in_enc_skey;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::generate(int in_nid)
{
	DEBUGC_STREAM_CALLED;

	if (ec_key != NULL)
		ECKey_Free(ec_key);
	if (!ECKey_NewKey(in_nid, (void **)(&ec_key), prvkey_oct, pubkey.x, pubkey.y, pubkey.comp_key, pubkey.comp_key_mode))   {
		ERROR_STREAMC << "failed to generate new EC key" << std::endl;
		return false;
	}
	pubkey.nid = in_nid;

	dump_ttcn_object(prvkey_oct, "PrvKey raw: ");
	dump_ttcn_object(pubkey.x, "PrvKey X:");
	dump_ttcn_object(pubkey.y, "PrvKey Y:");
	dump_ttcn_object(pubkey.comp_key, "PrvKey CompPubKey:");
	dump_ttcn_object(pubkey.comp_key_mode, "PrvKey CompPubKeyMode:");
	DEBUGC_STREAM << "PrvKey NID:" <<  pubkey.nid << std::endl;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::checkDecryptContext(OCTETSTRING &skey_id8)
{
	DEBUGC_STREAM_CALLED;

	if (!skey_id8.is_bound() || !aes_skey.is_bound())   {
		ERROR_STREAMC << "invalid Decrypt context" << std::endl;
		return false;
	}

	IEEE1609dot2BaseTypes::SymmetricEncryptionKey skey_type;
        skey_type.aes128Ccm() = aes_skey;

	OCTETSTRING skey_encoded;
	if (!encSymmetricEncryptionKey(skey_type, skey_encoded))   {
		ERROR_STREAMC << "failed to encode IEEE1609dot2BaseTypes::SymmetricEncryptionKey" << std::endl;
		return false;
	}

	OCTETSTRING aes_key_id8;
	if (!hash_256_id8(skey_encoded, aes_key_id8))   {
		ERROR_STREAMC << "cannot get sha256 hashed ID8" << std::endl;
		return false;
	}

	if (aes_key_id8 != skey_id8)   {
		ERROR_STREAMC << "invalid AES key" << std::endl;
		return false;
	}

	if (!tag.is_bound() || tag.lengthof() == 0)   {
		ERROR_STREAMC << "no TAG in decryption context" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::decryptAes128ccm(IEEE1609dot2::AesCcmCiphertext &aes128ccm, OCTETSTRING &ret)
{
	DEBUGC_STREAM_CALLED;

	if (!aes128ccm.is_bound())   {
		ERROR_STREAMC << "invalid argument" << std::endl;
		return false;
	}

        int tag_len = tag.lengthof();
	OCTETSTRING tag(tag_len, (const unsigned char *)aes128ccm.ccmCiphertext() + aes128ccm.ccmCiphertext().lengthof() - tag_len);

	OCTETSTRING ctext(aes128ccm.ccmCiphertext().lengthof() - tag_len, (const unsigned char *)aes128ccm.ccmCiphertext());

	dump_ttcn_object(aes_skey, "AES SKey: ");
	if (!OpenSSL_Decrypt_aes128ccm(ctext, tag, aes128ccm.nonce(), aes_skey, ret))   {
		ERROR_STREAMC << "AES-128-CCM decrypt error" << std::endl;
		return false;
	}
	dump_ttcn_object(ret, "Decrypted payload: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::derivate(const OCTETSTRING &x, const OCTETSTRING &y, const OCTETSTRING &salt)
{
	DEBUGC_STREAM_CALLED;

	if (!IsValid())   {
		ERROR_STREAMC << "private key data are not valid" << std::endl;
		return false;
	}

	if (!ECKey_DerivateSKey_aes128ccm(ec_key, x, y, salt, aes_skey, enc_skey, tag))   {
		ERROR_STREAMC << "Cannot derivate SKey" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::GetP256CurvePoint(IEEE1609dot2BaseTypes::EccP256CurvePoint &ret)
{
	DEBUGC_STREAM_CALLED;

	if (pubkey.GetCompressedMode() == 0)   {
		if (!pubkey.GetCompressed(ret.compressed__y__0()))   {
			ERROR_STREAMC << "cannot get compressed Y0 component" << std::endl;
			return false;
		}
	}
	else if (pubkey.GetCompressedMode() == 1)   {
		if (!pubkey.GetCompressed(ret.compressed__y__1()))   {
			ERROR_STREAMC << "cannot get compressed Y1 component" << std::endl;
			return false;
		}
	}
	else   {
		if (!pubkey.GetXY(ret.uncompressedP256().x(), ret.uncompressedP256().y()))   {
			ERROR_STREAMC << "cannot get uncompressed components" << std::endl;
			return false;
		}
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::GetEciesP256EncryptedKey(IEEE1609dot2BaseTypes::EciesP256EncryptedKey &ret)
{
	DEBUGC_STREAM_CALLED;

	IEEE1609dot2BaseTypes::EccP256CurvePoint eccP256CurvePoint;
	if (!GetP256CurvePoint(eccP256CurvePoint))   {
		ERROR_STREAMC << "cannot get IEEE1609dot2BaseTypes::EccP256CurvePoint" << std::endl;
		return false;
	}

	if (!enc_skey.is_present())   {
		ERROR_STREAMC << "invalid context: enc_skey is missing" << std::endl;
		return false;
	}

	if (!tag.is_present())   {
		ERROR_STREAMC << "invalid context: tag is missing" << std::endl;
		return false;
	}

	ret = IEEE1609dot2BaseTypes::EciesP256EncryptedKey(eccP256CurvePoint, enc_skey, tag);

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::GetEncryptedDataEncryptionKey(IEEE1609dot2::EncryptedDataEncryptionKey &ret)
{
	DEBUGC_STREAM_CALLED;

	IEEE1609dot2BaseTypes::EciesP256EncryptedKey ecies_key;
	if (!GetEciesP256EncryptedKey(ecies_key))   {
		ERROR_STREAMC << "failed to get IEEE1609dot2BaseTypes::EciesP256EncryptedKey" << std::endl;
		return false;
	}

	switch (pubkey.nid)  {
	case NID_X9_62_prime256v1:
		ret.eciesNistP256() = ecies_key;
		break;
	case NID_brainpoolP256r1:
		ret.eciesBrainpoolP256r1() = ecies_key;
		break;
	default:
		ERROR_STREAMC << "EC curve '" << pubkey.nid << "' do not supported" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::encrypt(const OCTETSTRING &msg, OCTETSTRING &nonce, OCTETSTRING &enc_msg)
{
	DEBUGC_STREAM_CALLED;

	nonce = random_OCTETSTRING(Ieee1609Dot2Data_AesCcmCiphertext_NonceLength);
	switch (enc_algorithm) {
	case NID_aes_128_ccm:
		if (!OpenSSL_Encrypt_aes128ccm(msg, nonce, aes_skey, tag, enc_msg))   {
			ERROR_STREAMC << "OpenSSL encrypt AES 128 CCM failed" << std::endl;
			return false;
		}
		break;
	default:
		ERROR_STREAMC << "encryption algorithm '" << enc_algorithm << "' do not supported" << std::endl;
		return false;
	}

	dump_ttcn_object(aes_skey, "Encrypt(): AES SKey: ");
	dump_ttcn_object(nonce, "Encrypt(): Nonce: ");
	dump_ttcn_object(tag, "Encrypt(): Tag: ");
	OCTETSTRING _msg = msg;
	dump_ttcn_object(_msg, "Encrypt(): Message: ");
	dump_ttcn_object(enc_msg, "Encrypt(): Encrypted Message: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::encrypt(const OCTETSTRING &msg, IEEE1609dot2::AesCcmCiphertext &cipher_txt)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING nonce, enc_msg;
	if (!this->encrypt(msg, nonce, enc_msg))   {
		ERROR_STREAMC << "encrypt failed" << std::endl;
		return false;
	}

	cipher_txt = IEEE1609dot2::AesCcmCiphertext(nonce, enc_msg + tag); // Add tag at the end of the ciphered text
	dump_ttcn_object(cipher_txt, "AesCcmCiphertext: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::ItsPkiPrivateKey::encrypt(const OCTETSTRING &msg, IEEE1609dot2::SymmetricCiphertext &sym_cipher_txt)
{
	DEBUGC_STREAM_CALLED;

	IEEE1609dot2::AesCcmCiphertext aes_128_ccm;
	if (!this->encrypt(msg, aes_128_ccm))   {
		ERROR_STREAMC << "encrypt message failed" << std::endl;
		return false;
	}
	dump_ttcn_object(aes_128_ccm, "Encrypted message: ");

	sym_cipher_txt.aes128ccm() = aes_128_ccm;
	dump_ttcn_object(sym_cipher_txt, "SymmetricCiphertext: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::EncryptPayload(OCTETSTRING &tbe, EtsiTs103097Module::EtsiTs103097Data__Encrypted__My &ret)
{
	DEBUGC_STREAM_CALLED;

	if (!ready)   {
                ERROR_STREAMC << "error: etsi service is not properly initialized" << std::endl;
		return false;
	}

#if 0
	if (!enc_key.setup(GetRecipientEncryptionNID(), "BCF939F56A94FC275CD0AE6BD2165D62BA58113EA0B22C3FE14956470C9B503D"))
		return false;
#else
	if (!enc_key.generate(GetRecipientEncryptionNID()))   {
                ERROR_STREAMC << "failed to generate encryption key" << std::endl;
		return false;
	}

#endif

	OCTETSTRING r_enc_pubkey_x, r_enc_pubkey_y;
	if (!recipient.GetEncryptionXY(r_enc_pubkey_x, r_enc_pubkey_y))   {
                ERROR_STREAMC << "failed to get recipient encryption XY components" << std::endl;
		return false;
	}

	OCTETSTRING r_cert_hash;
	if (!recipient.GetCertHash(r_cert_hash))   {
                ERROR_STREAMC << "failed to get recipient certificate's hash" << std::endl;
		return false;
	}

	dump_ttcn_object(r_enc_pubkey_x, "Recipient pubkey X: ");
	dump_ttcn_object(r_enc_pubkey_y, "Recipient pubkey Y: ");
	dump_ttcn_object(r_cert_hash, "Recipient Cert Hash: ");
	if (!enc_key.derivate(r_enc_pubkey_x, r_enc_pubkey_y, r_cert_hash))   {
                ERROR_STREAMC << "derivate failed" << std::endl;
		return false;
	}

	IEEE1609dot2::EncryptedDataEncryptionKey enc_data_key;
	if (!enc_key.GetEncryptedDataEncryptionKey(enc_data_key))   {
                ERROR_STREAMC << "cannot get IEEE1609dot2::EncryptedDataEncryptionKey" << std::endl;
		return false;
	}
	dump_ttcn_object(enc_data_key, "EncryptedDataEncryptionKey: ");

	//     cipher_text := { aes128ccm := { nonce := ''O, ccmCiphertext := ''O } }
	IEEE1609dot2::SymmetricCiphertext cipher_text;
	if (!enc_key.encrypt(tbe, cipher_text))   {
                ERROR_STREAMC << "encrypt cipher text failed" << std::endl;
		return false;
	}
	dump_ttcn_object(cipher_text, "Encrypted payload: ");

	OCTETSTRING cipher_txt_oct;
	if (!encSymmetricCiphertext(cipher_text, cipher_txt_oct))   {
                ERROR_STREAMC << "encode SymmetricCiphertext failed" << std::endl;
		return false;
	}
	dump_ttcn_object(cipher_txt_oct, "Encoded Encrypted payload: ");

	OCTETSTRING recipient_id;
	if (!recipient.GetHashedID8(recipient_id))   {
                ERROR_STREAMC << "cannot get recipiet hashed ID8" << std::endl;
		return false;
	}
	dump_ttcn_object(recipient_id, "Recipient ID: ");

	//  certRecipInfo := {
	//    recipientId := ''O,
	//    encKey := { eciesNistP256 := { v := { compressed_y_0 := ''O }, c := ''O, t := ''O } }
	//  }
	IEEE1609dot2::PKRecipientInfo cert_recipient_info(recipient_id, enc_data_key);
	IEEE1609dot2::RecipientInfo recipient_info;
	recipient_info.certRecipInfo() = cert_recipient_info;
	IEEE1609dot2::SequenceOfRecipientInfo recipients;
	recipients[0] = recipient_info;

	// encryptedData := {
	//     recipients := { { certRecipInfo := { recipientId := ''O, encKey := { eciesNistP256 := { v := { compressed_y_0 := ''O }, c := ''O, t := ''O } } } } },
	//     ciphertext := { aes128ccm := { nonce := ''O, ccmCiphertext := ''O } }
	// }
	IEEE1609dot2::EncryptedData encrypted_data(recipients, cipher_text);
	dump_ttcn_object(encrypted_data, "EncryptedData:  ");

	IEEE1609dot2::Ieee1609Dot2Content ieee_dot2_content;
 	ieee_dot2_content.encryptedData() = encrypted_data;

	// { protocolVersion := 3,
	//   content := { encryptedData := {
	//      recipients := { { certRecipInfo := { recipientId := ''O, encKey := { eciesNistP256 := { v := { compressed_y_0 := ''O }, c := ''O, t := ''O } } } } },
	//      ciphertext := { aes128ccm := { nonce := ''O, ccmCiphertext := ''O } } } } }
	ret = IEEE1609dot2::Ieee1609Dot2Data(Ieee1609Dot2Data_ProtocolVersion, ieee_dot2_content);
	dump_ttcn_object(ret, "EncryptedData Payload: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiEtsi::DecryptPayload(OCTETSTRING &in_raw, OCTETSTRING &payload)
{
	DEBUGC_STREAM_CALLED;

	if (!ready)   {
                ERROR_STREAMC << "error: etsi service is not properly initialized" << std::endl;
		return false;
	}

        // { protocolVersion := 3, content := { encryptedData := { recipients := { { pskRecipInfo := ''O } }, ciphertext := { aes128ccm := { nonce := ''O, ccmCiphertext := ''O } } } } }
        EtsiTs103097Module::EtsiTs103097Data__Encrypted__My data_encrypted;
	try   {
		OCTETSTRING _in_raw = in_raw;
        	EtsiTs103097Module::EtsiTs103097Data__Encrypted__My_decoder(_in_raw, data_encrypted, "OER");
	}
	catch (const TC_Error& tc_error) {
		bool ascii = true;
		for (int ii = 0; ii < 200 && ii < in_raw.lengthof(); ii++)   {
			char ch = *((char *)(const unsigned char *)in_raw + ii);
			if (!isascii((ch)))   {
				ascii = false;
				break;
			}
		}

		ERROR_STREAMC << "cannot decode payload: " << (ascii ? (char *)((const unsigned char *)in_raw) : oct2str(in_raw)) << std::endl;
		return false;
	}

	return DecryptPayload(data_encrypted, payload);
}

bool
ItsPkiEtsi::DecryptPayload(EtsiTs103097Module::EtsiTs103097Data__Encrypted__My &data_encrypted, OCTETSTRING &payload)
{
	DEBUGC_STREAM_CALLED;

        // { protocolVersion := 3, content := { encryptedData := { recipients := { { pskRecipInfo := ''O } }, ciphertext := { aes128ccm := { nonce := ''O, ccmCiphertext := ''O } } } } }
	if (!ready)   {
                ERROR_STREAMC << "error: etsi service is not properly initialized" << std::endl;
		return false;
	}

        if (!data_encrypted.is_bound()) {
                ERROR_STREAMC << "cannot decode response encrypted data" << std::endl;
                return false;
        }
        dump_ttcn_object(data_encrypted, "EtsiTs103097Module::EtsiTs103097Data__Encrypted__My response: ");

        if (!data_encrypted.content().ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_encryptedData))   {
                ERROR_STREAMC << "'"<< data_encrypted.content().get_selection() << "' content type instead of expected 'ALT_encryptedData'" << std::endl;
                return false;
        }

        IEEE1609dot2::RecipientInfo rinfo = data_encrypted.content().encryptedData().recipients()[0];
	if (rinfo.ischosen(IEEE1609dot2::RecipientInfo::ALT_certRecipInfo))   {
		if (recipient.GetPrivateKey() == NULL)   {
                	ERROR_STREAMC << "No recipient private key" << std::endl;
                	return false;
		}

		OCTETSTRING recipient_id;
		if (!this->recipient.GetHashedID8(recipient_id))   {
                	ERROR_STREAMC << "Cannot get HashedID " << std::endl;
                	return false;
		}

		if (recipient_id != rinfo.certRecipInfo().recipientId())   {
			ERROR_STREAMC << "No recipient with id '" << oct2str(recipient_id) << std::endl;
			return false;
		}

		int nid;
		IEEE1609dot2BaseTypes::EciesP256EncryptedKey ecies_key;
		if (rinfo.certRecipInfo().encKey().ischosen(IEEE1609dot2::EncryptedDataEncryptionKey::ALT_eciesNistP256)) {
			ecies_key = rinfo.certRecipInfo().encKey().eciesNistP256();
			nid = NID_X9_62_prime256v1;
		}

		else if (rinfo.certRecipInfo().encKey().ischosen(IEEE1609dot2::EncryptedDataEncryptionKey::ALT_eciesBrainpoolP256r1)) {
			ecies_key = rinfo.certRecipInfo().encKey().eciesBrainpoolP256r1();
			nid = NID_brainpoolP256r1;
		}
		else {
			ERROR_STREAMC << "Unsupported type of ECIES encrypted key " << std::endl;
			return false;
		}

        	OCTETSTRING comp, x, y;
        	int y_bit = -1;
        	if (ecies_key.v().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
                	x = ecies_key.v().uncompressedP256().x();
                	y = ecies_key.v().uncompressedP256().y();
        	}
        	else if (ecies_key.v().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0))   {
                	comp = ecies_key.v().compressed__y__0();
                	y_bit = 0;

                	if (!ECKey_DecompressPublicKey(nid, comp, 0, x, y))   {
                        	ERROR_STREAMC << "Cannot decompress EC PublicKey" << std::endl;
                        	return false;
                	}
        	}
        	else if (ecies_key.v().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1))   {
                	comp = ecies_key.v().compressed__y__1();
                	y_bit = 1;
                	if (!ECKey_DecompressPublicKey(nid, comp, 1, x, y))   {
                        	ERROR_STREAMC << "Cannot decompress EC PublicKey" << std::endl;
                        	return false;
                	}
        	}
        	else   {
                	ERROR_STREAMC << "Unsupported EC key form" << std::endl;
                	return false;
        	}
        	dump_ttcn_object(x, "Sender X: ");
        	dump_ttcn_object(y, "Sender Y: ");

		OCTETSTRING salt;
		if (!recipient.GetCertHash(salt))   {
                	ERROR_STREAMC << "Failed to get certificate hash " << std::endl;
                	return false;
		}

		enc_key.setup(recipient.GetPrivateKey(), ecies_key.c());
		if (!enc_key.derivate(x, y, salt))   {
                	ERROR_STREAMC << "Cannot derivate " << std::endl;
                	return false;
		}
	}
	else if (rinfo.ischosen(IEEE1609dot2::RecipientInfo::ALT_pskRecipInfo))  {
        	OCTETSTRING skey_id = rinfo.pskRecipInfo();
		if (!setDecryptContextWithSKey(skey_id))   {
                	ERROR_STREAMC << "cannot setup decrypt context" << std::endl;
                	return false;
        	}
	}
	else   {
                ERROR_STREAMC << "'" << rinfo.get_selection() << "' recipient type, instead of expected 'ALT_pskRecipInfo'" << std::endl;
                return false;
        }

        IEEE1609dot2::AesCcmCiphertext aes128ccm = data_encrypted.content().encryptedData().ciphertext().aes128ccm();
        dump_ttcn_object(aes128ccm, "IEEE1609dot2::AesCcmCiphertext: ");

        if (!enc_key.decryptAes128ccm(aes128ccm, payload))   {
                ERROR_STREAMC << "decrypt with Aes128CCM failed" << std::endl;
                return false;
        }

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}


#ifndef TESTS_TEST_SOLVER_HPP
#define TESTS_TEST_SOLVER_HPP

#include <amgcl/amg.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/adapter/zero_copy.hpp>
#include <amgcl/profiler.hpp>

#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

#include "sample_problem.hpp"

namespace amgcl {
    profiler<> prof;
}

//---------------------------------------------------------------------------
template <class Backend, class Matrix>
void test_solver(
        const Matrix &A,
        std::shared_ptr<typename Backend::vector> const &f,
        std::shared_ptr<typename Backend::vector>       &x,
        amgcl::runtime::solver::type     solver,
        amgcl::runtime::relaxation::type relaxation,
        amgcl::runtime::coarsening::type coarsening,
        bool test_null_space = false
        )
{
    boost::property_tree::ptree prm;
    prm.put("precond.coarse_enough",   500);
    prm.put("precond.coarsening.type", coarsening);
    prm.put("precond.relax.type",      relaxation);
    prm.put("solver.type",             solver);

    typedef typename Backend::value_type value_type;
    std::vector<value_type> null;

    if (test_null_space) {
        size_t n = amgcl::backend::rows(*A);
        null.resize(n, amgcl::math::constant<value_type>(1));

        prm.put("precond.coarsening.nullspace.cols", 1);
        prm.put("precond.coarsening.nullspace.rows", n);
        prm.put("precond.coarsening.nullspace.B",    &null[0]);
    }

    amgcl::make_solver<
        amgcl::amg<Backend, amgcl::runtime::coarsening::wrapper, amgcl::runtime::relaxation::wrapper>,
        amgcl::runtime::solver::wrapper<Backend>
        > solve(A, prm);

    std::cout << solve.precond() << std::endl;

    size_t iters;
    double resid;

    amgcl::backend::clear(*x);

    std::tie(iters, resid) = solve(*f, *x);

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << resid << std::endl
              << std::endl;

    BOOST_REQUIRE_SMALL(resid, 1e-4);
}

//---------------------------------------------------------------------------
template <class Backend, class Matrix>
void test_rap(
        const Matrix &A,
        std::shared_ptr<typename Backend::vector> const &f,
        std::shared_ptr<typename Backend::vector>       &x,
        amgcl::runtime::solver::type     solver,
        amgcl::runtime::relaxation::type relaxation
        )
{
    boost::property_tree::ptree prm;
    prm.put("precond.type", relaxation);
    prm.put("solver.type",  solver);

    amgcl::make_solver<
        amgcl::relaxation::as_preconditioner<Backend, amgcl::runtime::relaxation::wrapper>,
        amgcl::runtime::solver::wrapper<Backend>
        > solve(A, prm);

    std::cout << "Using " << relaxation << " as preconditioner" << std::endl;

    size_t iters;
    double resid;

    amgcl::backend::clear(*x);

    std::tie(iters, resid) = solve(*f, *x);

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << resid << std::endl
              << std::endl;

    BOOST_CHECK_SMALL(resid, 1e-4);
}

template <class Backend, class value_type, class rhs_type>
void test_problem(
        size_t n,
        std::vector<ptrdiff_t>  ptr,
        std::vector<ptrdiff_t>  col,
        std::vector<value_type> val,
        std::vector<rhs_type>   rhs
        )
{
    amgcl::runtime::coarsening::type coarsening[] = {
        amgcl::runtime::coarsening::aggregation,
        amgcl::runtime::coarsening::smoothed_aggregation,
        amgcl::runtime::coarsening::smoothed_aggr_emin,
        amgcl::runtime::coarsening::ruge_stuben
    };

    amgcl::runtime::relaxation::type relaxation[] = {
        amgcl::runtime::relaxation::spai0
#ifndef AMGCL_RUNTIME_DISABLE_SPAI1
      , amgcl::runtime::relaxation::spai1
#endif
      , amgcl::runtime::relaxation::damped_jacobi
      , amgcl::runtime::relaxation::gauss_seidel
      , amgcl::runtime::relaxation::ilu0
      , amgcl::runtime::relaxation::iluk
      , amgcl::runtime::relaxation::ilut
#ifndef AMGCL_RUNTIME_DISABLE_CHEBYSHEV
      , amgcl::runtime::relaxation::chebyshev
#endif
    };

    amgcl::runtime::solver::type solver[] = {
        amgcl::runtime::solver::cg,
        amgcl::runtime::solver::bicgstab,
        amgcl::runtime::solver::bicgstabl,
        amgcl::runtime::solver::gmres,
        amgcl::runtime::solver::lgmres,
        amgcl::runtime::solver::fgmres,
        amgcl::runtime::solver::idrs
    };

    typename Backend::params prm;

    auto y = Backend::copy_vector(rhs, prm);
    auto x = Backend::create_vector(n, prm);

    // Test solvers
    for(amgcl::runtime::solver::type s : solver) {
        std::cout << "Solver: " << s << std::endl;
        try {
            test_solver<Backend>(
                    amgcl::adapter::zero_copy(n, ptr.data(), col.data(), val.data()),
                    y, x, s, relaxation[0], coarsening[0]
                    );
        } catch(const std::logic_error&) {}
    }

    // Test smoothers
    for(amgcl::runtime::relaxation::type r : relaxation) {
        std::cout << "Relaxation: " << r << std::endl;
        try {
            test_solver<Backend>(
                    amgcl::adapter::zero_copy(n, ptr.data(), col.data(), val.data()),
                    y, x, solver[0], r, coarsening[0]);
        } catch(const std::logic_error&) {}

        try {
            std::cout << "Relaxation as preconditioner: " << r << std::endl;

            test_rap<Backend>(
                    amgcl::adapter::zero_copy(n, ptr.data(), col.data(), val.data()),
                    y, x, solver[0], r);
        } catch(const std::logic_error&) {}
    }

    // Test coarsening
    for(amgcl::runtime::coarsening::type c : coarsening) {
        std::cout << "Coarsening: " << c << std::endl;

        try {
            test_solver<Backend>(
                    amgcl::adapter::zero_copy(n, ptr.data(), col.data(), val.data()),
                    y, x, solver[0], relaxation[0], c);
        } catch(const std::logic_error&) {}

        switch (c) {
            case amgcl::runtime::coarsening::aggregation:
            case amgcl::runtime::coarsening::smoothed_aggregation:
            case amgcl::runtime::coarsening::smoothed_aggr_emin:
                test_solver<Backend>(
                        amgcl::adapter::zero_copy(n, ptr.data(), col.data(), val.data()),
                        y, x, solver[0], relaxation[0], c, /*test_null_space*/true);
                break;
            default:
                break;
        }
    }
}

template <class Backend>
void test_backend() {
    typedef typename Backend::value_type value_type;
    typedef typename amgcl::math::rhs_of<value_type>::type rhs_type;

    // Poisson 3D
    {
        std::vector<ptrdiff_t>  ptr;
        std::vector<ptrdiff_t>  col;
        std::vector<value_type> val;
        std::vector<rhs_type>   rhs;

        size_t n = sample_problem(32, val, col, ptr, rhs);

        test_problem<Backend>(n, ptr, col, val, rhs);
    }

    // Trivial problem
#if !defined(SOLVER_BACKEND_VIENNACL)
    {
        std::vector<ptrdiff_t>  ptr;
        std::vector<ptrdiff_t>  col;
        std::vector<value_type> val;
        std::vector<rhs_type>   rhs;

	val += amgcl::math::identity<value_type>(), amgcl::math::identity<value_type>();
	col += 0, 1;
	ptr += 0, 1, 2;
	rhs += amgcl::math::constant<rhs_type>(1.0), amgcl::math::zero<rhs_type>();

	size_t n = rhs.size();

        test_problem<Backend>(n, ptr, col, val, rhs);
    }
#endif
}

#endif


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_TREXC_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_TREXC_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Reorder the Schur factorization of a matrix.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * trexc()  reorders the Schur factorization of a matrix
     * A =  Q*T*Q**T, so that the diagonal block of T with row
     * index IFST is  moved to row ILST.
     */

    namespace detail {
      inline
      integer_t trexc_work_size( integer_t const n, float ) {return n;}

      inline
      integer_t trexc_work_size( integer_t const n, double ) {return n;}

      inline
      integer_t trexc_work_size( integer_t const n, traits::complex_f ) {return 0;}

      inline
      integer_t trexc_work_size( integer_t const n, traits::complex_d ) {return 0;}
    }

    // Get the minimum size of the work array.
    template <typename MatrT>
    integer_t trexc_work_size(const MatrT& t) {
       return detail::trexc_work_size( traits::matrix_size1(t), typename MatrT::value_type() );
    }

    namespace detail {
      inline
      void trexc (char const compq, integer_t const n,
                 float* t, integer_t const ldt, float* q, integer_t const ldq, integer_t& ifst, integer_t& ilst,
                 float* work, integer_t& info)
      {
        LAPACK_STREXC (&compq, &n, t, &ldt, q, &ldq, &ifst, &ilst, work, &info);
      }

      inline
      void trexc (char const compq, integer_t const n,
                 double* t, integer_t const ldt, double* q, integer_t const ldq, integer_t& ifst, integer_t& ilst,
                 double* work, integer_t& info)
      {
        LAPACK_DTREXC (&compq, &n, t, &ldt, q, &ldq, &ifst, &ilst, work, &info);
      }

      inline
      void trexc (char const compq, integer_t const n,
                 traits::complex_f* t, integer_t const ldt, traits::complex_f* q, integer_t const ldq, integer_t& ifst, integer_t& ilst,
                 float* work, integer_t& info)
      {
        LAPACK_CTREXC (&compq, &n, traits::complex_ptr(t), &ldt, traits::complex_ptr(q), &ldq, &ifst, &ilst, &info);
      }

      inline
      void trexc (char const compq, integer_t const n,
                 traits::complex_d* t, integer_t const ldt, traits::complex_d* q, integer_t const ldq, integer_t& ifst, integer_t& ilst,
                 double* work, integer_t& info)
      {
        LAPACK_ZTREXC (&compq, &n, traits::complex_ptr(t), &ldt, traits::complex_ptr(q), &ldq, &ifst, &ilst, &info);
      }

    }

    // Reorder Schur factorization with Schur vectors
    template <typename MatrT, typename Q, typename Work>
    int trexc (char const compq, MatrT& t, Q& q, integer_t& ifst, integer_t& ilst, Work& work) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrT>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<Q>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (t);
      assert (n == traits::matrix_size2 (t));
      assert (n == traits::matrix_size1 (q));
      assert (n == traits::matrix_size2 (q));
      assert (trexc_work_size(t) <= traits::vector_size (work));

      integer_t info;
      detail::trexc (compq, n,
                    traits::matrix_storage (t),
                    traits::leading_dimension (t),
                    traits::matrix_storage (q),
                    traits::leading_dimension (q),
                    ifst, ilst,
                    traits::vector_storage (work),
                    info);
      return info;
    }

  }

}}}

#endif


#include "util/statistics.hpp"

#include <map>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/error_of.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
using namespace boost::accumulators;

static uint32_t handles = 0;
static std::map<uint32_t, void *> db;

typedef accumulator_set<double, stats<tag::mean, tag::error_of<tag::mean>, tag::variance(lazy) > > stat_mean_t;

typedef accumulator_set<double, stats<tag::density > > stat_histo_t;
typedef boost::iterator_range<std::vector<std::pair<double, double> >::iterator > stat_histo_iter_t;

typedef accumulator_set<double, stats<tag::tail_quantile<left> > > stat_tail_left_t;
typedef accumulator_set<double, stats<tag::tail_quantile<right> > > stat_tail_right_t;

uint32_t init_mean_stat() {
  db[handles] = (void *)(new stat_mean_t());
  return handles++;
}

uint32_t init_histo_stat(uint32_t binN, uint32_t cacheS) {
  db[handles] = (void *)(new stat_histo_t(tag::density::num_bins = binN,
                                             tag::density::cache_size = cacheS));
  return handles++;
}

uint32_t init_tail_stat(bool dir, uint32_t cacheS) {
  if(dir)
    db[handles] = (void *)(new stat_tail_left_t(tag::tail<left>::cache_size = cacheS));
  else
    db[handles] = (void *)(new stat_tail_right_t(tag::tail<right>::cache_size = cacheS));
  return handles++;
}

void record_mean_stat(uint32_t handle, double sample) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  (*stat)(sample);
}

void record_histo_stat(uint32_t handle, double sample) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  (*stat)(sample);
}

void record_tail_stat(uint32_t handle, bool dir, double sample) {
  assert(db.count(handle));
  if(dir) {
    stat_tail_right_t *stat = (stat_tail_right_t *)(db[handle]);
    (*stat)(sample);
  } else {
    stat_tail_left_t *stat = (stat_tail_left_t *)(db[handle]);
    (*stat)(sample);
  }
}

uint32_t get_mean_count(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return count(*stat);
}

double get_mean_mean(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return mean(*stat);
}

double get_mean_error(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return error_of<tag::mean>(*stat);
}

double get_mean_variance(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  return variance(*stat);
}

uint32_t get_histo_count(uint32_t handle) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  return count(*stat);
}

std::vector<std::pair<double, double> > get_histo_density(uint32_t handle) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  stat_histo_iter_t hist = density(*stat);
  std::vector<std::pair<double, double> > rv(hist.size());
  for(uint32_t i=0; i<hist.size(); i++) rv[i] = hist[i];
  return rv;
}

double get_tail_quantile(uint32_t handle, bool dir, double ratio) {
  assert(db.count(handle));
  if(dir) {
    stat_tail_right_t *stat = (stat_tail_right_t *)(db[handle]);
    assert(ratio >= 0.5);
    return quantile(*stat, quantile_probability = ratio);
  } else {
    stat_tail_left_t *stat = (stat_tail_left_t *)(db[handle]);
    assert(ratio <= 0.5);
    return quantile(*stat, quantile_probability = ratio);
  }
}

void close_mean_stat(uint32_t handle) {
  assert(db.count(handle));
  stat_mean_t *stat = (stat_mean_t *)(db[handle]);
  delete stat;
  db.erase(handle);
}

void close_histo_stat(uint32_t handle) {
  assert(db.count(handle));
  stat_histo_t *stat = (stat_histo_t *)(db[handle]);
  delete stat;
  db.erase(handle);
}

void close_tail_stat(uint32_t handle, bool dir) {
  assert(db.count(handle));
  if(dir) {
    stat_tail_right_t *stat = (stat_tail_right_t *)(db[handle]);
    delete stat;
  } else {
    stat_tail_left_t *stat = (stat_tail_left_t *)(db[handle]);
    delete stat;
  }
  db.erase(handle);
}


#include <Eigen/Dense>
#include "edge_labeler.h"
#include "g2o/stuff/unscented.h"

namespace g2o {

  using namespace std;
  using namespace Eigen;

  typedef SigmaPoint<VectorXd> MySigmaPoint;

  EdgeLabeler::EdgeLabeler(SparseOptimizer* optimizer) {
    _optimizer = optimizer;
  }

  int EdgeLabeler::labelEdges(std::set<OptimizableGraph::Edge*>& edges){
    // assume the system is "solved"
    // compute the sparse pattern of the inverse
    std::set<std::pair<int, int> > pattern;
    for (std::set<OptimizableGraph::Edge*>::iterator it=edges.begin(); it!=edges.end(); it++){
      augmentSparsePattern(pattern, *it);
    }

    SparseBlockMatrix<MatrixXd> spInv;

    bool result = computePartialInverse(spInv, pattern);
    //cerr << "partial inverse computed = " << result << endl;
    //cerr << "non zero blocks" << spInv.nonZeroBlocks() << endl;

    if (! result ){
      return -1;
    }
    int count=0;
    for (std::set<OptimizableGraph::Edge*>::iterator it=edges.begin(); it!=edges.end(); it++){
      count += labelEdge(spInv, *it) ? 1 : 0;
    }
    return count;
  }

  void EdgeLabeler::augmentSparsePattern(std::set<std::pair<int, int> >& pattern, OptimizableGraph::Edge* e){
    for (size_t i=0; i<e->vertices().size(); i++){
      const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) e->vertices()[i];
      int ti=v->hessianIndex();
      if (ti==-1)
	continue;
      for (size_t j=i; j<e->vertices().size(); j++){
	const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) e->vertices()[j];
	int tj = v->hessianIndex();
	if (tj==-1)
	  continue;
	if(tj<ti)
	  swap(ti,tj);
	pattern.insert(std::make_pair(ti, tj));
      }
    }
  }

  bool EdgeLabeler::computePartialInverse(SparseBlockMatrix<MatrixXd>& spinv, const std::set<std::pair<int,int> >& pattern){
    std::vector<std::pair<int, int> > blockIndices(pattern.size());
    // Why this does not work???
    //std::copy(pattern.begin(),pattern.end(),blockIndices.begin());

    int k=0;
    for(std::set<std::pair<int, int> >::const_iterator it= pattern.begin(); it!=pattern.end(); it++){
      blockIndices[k++]=*it;
    }

    //cerr << "sparse pattern contains " << blockIndices.size() << " blocks" << endl;
    return _optimizer->computeMarginals(spinv, blockIndices);
  }

  bool EdgeLabeler::labelEdge( const SparseBlockMatrix<MatrixXd>& spinv, OptimizableGraph::Edge* e){

    Eigen::Map<MatrixXd> info(e->informationData(), e->dimension(), e->dimension());
    // cerr << "original information matrix" << endl;
    // cerr << info << endl;

    int maxDim=0;
    for (size_t i=0; i<e->vertices().size(); i++){
      const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) e->vertices()[i];
      int ti=v->hessianIndex();
      if (ti==-1)
	continue;
      maxDim+=v->minimalEstimateDimension();
    }

    //cerr << "maxDim= " << maxDim << endl;
    MatrixXd cov(maxDim, maxDim);
    int cumRow=0;
    for (size_t i=0; i<e->vertices().size(); i++){
      const OptimizableGraph::Vertex* vr=(const OptimizableGraph::Vertex*) e->vertices()[i];
      int ti=vr->hessianIndex();
      if (ti>-1) {
	int cumCol=0;
	for (size_t j=0; j<e->vertices().size(); j++){
	  const OptimizableGraph::Vertex* vc=(const OptimizableGraph::Vertex*) e->vertices()[j];
	  int tj = vc->hessianIndex();
	  if (tj>-1){
	    // cerr << "ti=" << ti << " tj=" << tj
	    //    << " cumRow=" << cumRow << " cumCol=" << cumCol << endl;
	    if (ti<=tj){
	      assert(spinv.block(ti, tj));
	      // cerr << "cblock_ptr" << spinv.block(ti, tj) << endl;
	      // cerr << "cblock.size=" << spinv.block(ti, tj)->rows() << "," << spinv.block(ti, tj)->cols() << endl;
	      // cerr << "cblock" << endl;
	      // cerr << *spinv.block(ti, tj) << endl;
	      cov.block(cumRow, cumCol, vr->minimalEstimateDimension(), vc->minimalEstimateDimension()) =
		*spinv.block(ti, tj);
	    } else {
	      assert(spinv.block(tj, ti));
	      // cerr << "cblock.size=" << spinv.block(tj, ti)->cols() << "," << spinv.block(tj, ti)->rows() << endl;
	      // cerr << "cblock" << endl;
	      // cerr << spinv.block(tj, ti)->transpose() << endl;
	      cov.block(cumRow, cumCol, vr->minimalEstimateDimension(), vc->minimalEstimateDimension()) =
		spinv.block(tj, ti)->transpose();
	    }
	    cumCol += vc->minimalEstimateDimension();
	  }
	}
	cumRow += vr->minimalEstimateDimension();
      }
    }

    // cerr << "covariance assembled" << endl;
    // cerr << cov << endl;
    // now cov contains the aggregate marginals of the state variables in the edge
    VectorXd incMean(maxDim);
    incMean.fill(0);
    std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > incrementPoints;
    if (! sampleUnscented(incrementPoints, incMean, cov)){
      cerr << "sampleUnscented fail" << endl;
      return false;
    }
    // now determine the zero-error measure by applying the error function of the edge
    // with a zero measurement
    // TODO!!!
    bool smss = e->setMeasurementFromState();
    if (! smss) {
      cerr << "FATAL: Edge::setMeasurementFromState() not implemented" << endl;
    }
    assert(smss && "Edge::setMeasurementFromState() not implemented");

    //std::vector<MySigmaPoint> globalPoints(incrementPoints.size());
    std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > errorPoints(incrementPoints.size());

    // for each sigma point, project it to the global space, by considering those variables
    // that are involved
    //cerr << "sigma points are extracted, remapping to measurement space" << endl;
    for (size_t i=0; i<incrementPoints.size(); i++) {
      int cumPos=0;
      //VectorXd globalPoint(maxDim);

      // push all the "active" state variables
      for (size_t j=0; j<e->vertices().size(); j++){
        OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
        int tj=vr->hessianIndex();
        if (tj==-1)
          continue;
        vr->push();
      }

      for (size_t j=0; j<e->vertices().size(); j++){
        OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
        int tj=vr->hessianIndex();
        if (tj==-1)
          continue;
        vr->oplus(&incrementPoints[i]._sample[cumPos]);
        //assert(vr->getMinimalEstimateData(&globalPoint[cumPos]) && "Vertex::getMinimalEstimateData(...) not implemented");
        cumPos+=vr->minimalEstimateDimension();
      }

      // construct the sigma point in the global space
      // globalPoints[i]._sample=globalPoint;
      // globalPoints[i]._wi=incrementPoints[i]._wi;
      // globalPoints[i]._wp=incrementPoints[i]._wp;

      // construct the sigma point in the error space
      e->computeError();
      Map<VectorXd> errorPoint(e->errorData(),e->dimension());

      errorPoints[i]._sample=errorPoint;
      errorPoints[i]._wi=incrementPoints[i]._wi;
      errorPoints[i]._wp=incrementPoints[i]._wp;

      // pop all the "active" state variables
      for (size_t j=0; j<e->vertices().size(); j++){
        OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
        int tj=vr->hessianIndex();
        if (tj==-1)
          continue;
        vr->pop();
      }

    }

    // reconstruct the covariance of the error by the sigma points
    MatrixXd errorCov(e->dimension(), e->dimension());
    VectorXd errorMean(e->dimension());
    reconstructGaussian(errorMean, errorCov, errorPoints);
    info=errorCov.inverse();

    // cerr << "remapped information matrix" << endl;
    // cerr << info << endl;
    return true;
  }

}


#include <CooTransformation/CooTransformation.hpp>

#include <boost/python.hpp>

namespace bp = boost::python;

bp::tuple PyXyz2LatLonRad(double dX, double dY, double dZ)
{
    double dLat, dLon, dRad;
    int nRet = Xyz2LatLonRad(dX, dY, dZ, &dLat, &dLon, &dRad);
    if (nRet != 0)
    {
        throw nRet;
    }
    return bp::make_tuple(dLat, dLon, dRad);
}

bp::tuple PyXyz2LatLonAlt(const char* pcPlanet, double dX, double dY, double dZ)
{
    double dLat, dLon, dAlt;
    int nRet = Xyz2LatLonAlt(pcPlanet, dX, dY, dZ, &dLat, &dLon, &dAlt);
    if (nRet != 0)
    {
        throw nRet;
    }
    return bp::make_tuple(dLat, dLon, dAlt);
}

bp::tuple PyLatLonAlt2Xyz(const char* pcPlanet, double dLat, double dLon, double dAlt)
{
    double dX, dY, dZ;
    int nRet = LatLonAlt2Xyz(pcPlanet, dLat, dLon, dAlt, &dX, &dY, &dZ);
    if (nRet != 0)
    {
        throw nRet;
    }
    return bp::make_tuple(dX, dY, dZ);
}


//
// Copyright (c) 2018-2019 CNRS INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);

  forwardKinematics(model,data,Model::ConfigVectorType::Ones(model.nq));
}

BOOST_AUTO_TEST_CASE(test_kinematics_zero)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);

  forwardKinematics(model,data_ref,q);
  crba(model,data,q);
  updateGlobalPlacements(model,data);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]);
    BOOST_CHECK(data.liMi[i] == data_ref.liMi[i]);
  }
}

BOOST_AUTO_TEST_CASE(test_kinematics_first)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Zero(model.nv));

  forwardKinematics(model,data,q,v);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.v[i] == Motion::Zero());
  }
}

BOOST_AUTO_TEST_CASE(test_kinematics_second)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Zero(model.nv));
  VectorXd a(VectorXd::Zero(model.nv));

  forwardKinematics(model,data,q,v,a);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.v[i] == Motion::Zero());
    BOOST_CHECK(data.a[i] == Motion::Zero());
  }
}

BOOST_AUTO_TEST_CASE(test_get_velocity)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));

  forwardKinematics(model,data,q,v);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i)));
    BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i,LOCAL)));
    BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model,data,i,WORLD)));
    BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.v[i]).isApprox(getVelocity(model,data,i,LOCAL_WORLD_ALIGNED)));
  }
}

BOOST_AUTO_TEST_CASE(test_get_acceleration)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  forwardKinematics(model,data,q,v,a);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i)));
    BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i,LOCAL)));
    BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model,data,i,WORLD)));
    BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.a[i]).isApprox(getAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
  }
}

BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  forwardKinematics(model,data,q,v,a);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    SE3 T = data.oMi[i];
    Motion vel = data.v[i];
    Motion acc = data.a[i];
    Vector3d linear;

    Motion acc_classical_local = acc;
    linear = acc.linear() + vel.angular().cross(vel.linear());
    acc_classical_local.linear() = linear;

    BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i)));
    BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i,LOCAL)));

    Motion vel_world = T.act(vel);
    Motion acc_classical_world = T.act(acc);
    linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
    acc_classical_world.linear() = linear;

    BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model,data,i,WORLD)));

    Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel);
    Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc);
    linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
    acc_classical_aligned.linear() = linear;

    BOOST_CHECK(acc_classical_aligned.isApprox(getClassicalAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
  }
}

BOOST_AUTO_TEST_CASE(test_kinematic_getters)
{
  using namespace Eigen;
  using namespace pinocchio;

  // Build a simple 2R planar model
  Model model;
  JointIndex jointId = 0;
  jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
  jointId = model.addJoint(jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");

  Data data(model);

  // Predetermined configuration values
  VectorXd q(model.nq);
  q << M_PI/2.0, 0.0;

  VectorXd v(model.nv);
  v << 1.0, 0.0;

  VectorXd a(model.nv);
  a << 0.0, 0.0;

  // Expected velocity
  Motion v_local;
  v_local.linear() = Vector3d(0.0, 1.0, 0.0);
  v_local.angular() = Vector3d(0.0, 0.0, 1.0);

  Motion v_world;
  v_world.linear() = Vector3d::Zero();
  v_world.angular() = Vector3d(0.0, 0.0, 1.0);

  Motion v_align;
  v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
  v_align.angular() = Vector3d(0.0, 0.0, 1.0);

  // Expected classical acceleration
  Motion ac_local;
  ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
  ac_local.angular() = Vector3d::Zero();

  Motion ac_world = Motion::Zero();

  Motion ac_align;
  ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
  ac_align.angular() = Vector3d::Zero();

  // Perform kinematics
  forwardKinematics(model,data,q,v,a);

  // Check output velocity
  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId)));
  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,LOCAL)));
  BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,WORLD)));
  BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,LOCAL_WORLD_ALIGNED)));

  // Check output acceleration (all zero)
  BOOST_CHECK(getAcceleration(model,data,jointId).isZero());
  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL).isZero());
  BOOST_CHECK(getAcceleration(model,data,jointId,WORLD).isZero());
  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED).isZero());

  // Check output classical
  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId)));
  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL)));
  BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,WORLD)));
  BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED)));
}

BOOST_AUTO_TEST_SUITE_END()