#include <Eigen/Dense>

#include <eigen-checks/gtest.h>
#include <maplab-common/test/testing-entrypoint.h>

#include <maplab-common/interpolation-helpers.h>
#include <vio-common/imu-measurements-buffer.h>
#include <vio-common/vio-types.h>

TEST(ImuMeasurementBuffer, PopFromEmptyBuffer) {
  vio_common::ImuMeasurementBuffer buffer(-1);

  // Pop from empty buffer.
  Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps(1, 2);
  Eigen::Matrix<double, 6, Eigen::Dynamic> imu_measurements(6, 2);
  vio_common::ImuMeasurementBuffer::QueryResult success =
      buffer.getImuDataInterpolatedBorders(
          50, 100, &imu_timestamps, &imu_measurements);

  EXPECT_EQ(
      success,
      vio_common::ImuMeasurementBuffer::QueryResult::kDataNotYetAvailable);
  EXPECT_EQ(0u, imu_timestamps.size());
  EXPECT_EQ(0u, imu_measurements.size());
}

TEST(ImuMeasurementBuffer, getImuDataInterpolatedBorders) {
  vio_common::ImuMeasurementBuffer buffer(-1);
  buffer.addMeasurement(10, vio::ImuData::Constant(10.0));
  buffer.addMeasurement(15, vio::ImuData::Constant(15.0));
  buffer.addMeasurement(20, vio::ImuData::Constant(20.0));
  buffer.addMeasurement(25, vio::ImuData::Constant(25.0));
  buffer.addMeasurement(30, vio::ImuData::Constant(30.0));
  buffer.addMeasurement(40, vio::ImuData::Constant(40.0));
  buffer.addMeasurement(50, vio::ImuData::Constant(50.0));

  Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps;
  Eigen::Matrix<double, 6, Eigen::Dynamic> imu_measurements;
  vio_common::ImuMeasurementBuffer::QueryResult result;

  // Test aligned getter (no-interpolation, only border values).
  result = buffer.getImuDataInterpolatedBorders(
      20, 30, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result, vio_common::ImuMeasurementBuffer::QueryResult::kDataAvailable);
  ASSERT_EQ(imu_timestamps.cols(), 3);
  ASSERT_EQ(imu_measurements.cols(), 3);
  EXPECT_EQ(imu_timestamps(0), 20);
  EXPECT_EQ(imu_timestamps(1), 25);
  EXPECT_EQ(imu_timestamps(2), 30);
  EXPECT_EQ(imu_measurements.col(0)(0), 20.0);
  EXPECT_EQ(imu_measurements.col(1)(0), 25.0);
  EXPECT_EQ(imu_measurements.col(2)(0), 30.0);

  // Test aligned getter (no-interpolation).
  result = buffer.getImuDataInterpolatedBorders(
      20, 40, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result, vio_common::ImuMeasurementBuffer::QueryResult::kDataAvailable);
  ASSERT_EQ(imu_timestamps.cols(), 4);
  ASSERT_EQ(imu_measurements.cols(), 4);
  EXPECT_EQ(imu_timestamps(0), 20);
  EXPECT_EQ(imu_timestamps(1), 25);
  EXPECT_EQ(imu_timestamps(2), 30);
  EXPECT_EQ(imu_timestamps(3), 40);
  EXPECT_EQ(imu_measurements.col(0)(0), 20.0);
  EXPECT_EQ(imu_measurements.col(1)(0), 25.0);
  EXPECT_EQ(imu_measurements.col(2)(0), 30.0);
  EXPECT_EQ(imu_measurements.col(3)(0), 40.0);

  // Test unaligned getter (lower/upper-interpolation).
  result = buffer.getImuDataInterpolatedBorders(
      19, 21, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result, vio_common::ImuMeasurementBuffer::QueryResult::kDataAvailable);
  ASSERT_EQ(imu_timestamps.cols(), 3);
  ASSERT_EQ(imu_measurements.cols(), 3);
  EXPECT_EQ(imu_timestamps(0), 19);
  EXPECT_EQ(imu_timestamps(1), 20);
  EXPECT_EQ(imu_timestamps(2), 21);
  EXPECT_EQ(imu_measurements.col(0)(0), 19.0);
  EXPECT_EQ(imu_measurements.col(1)(0), 20.0);
  EXPECT_EQ(imu_measurements.col(2)(0), 21.0);

  // Fail: query out of upper bound.
  result = buffer.getImuDataInterpolatedBorders(
      40, 51, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result,
      vio_common::ImuMeasurementBuffer::QueryResult::kDataNotYetAvailable);
  result = buffer.getImuDataInterpolatedBorders(
      60, 61, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result,
      vio_common::ImuMeasurementBuffer::QueryResult::kDataNotYetAvailable);

  // Fail: query out of lower bound.
  result = buffer.getImuDataInterpolatedBorders(
      -1, 20, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result,
      vio_common::ImuMeasurementBuffer::QueryResult::kDataNeverAvailable);
  result = buffer.getImuDataInterpolatedBorders(
      -20, -10, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result,
      vio_common::ImuMeasurementBuffer::QueryResult::kDataNeverAvailable);

  // Query between two values: return the border values.
  result = buffer.getImuDataInterpolatedBorders(
      21, 29, &imu_timestamps, &imu_measurements);
  ASSERT_EQ(
      result, vio_common::ImuMeasurementBuffer::QueryResult::kDataAvailable);
  ASSERT_EQ(imu_timestamps.cols(), 3);
  ASSERT_EQ(imu_measurements.cols(), 3);
  EXPECT_EQ(imu_timestamps(0), 21);
  EXPECT_EQ(imu_timestamps(1), 25);
  EXPECT_EQ(imu_timestamps(2), 29);
  EXPECT_EQ(imu_measurements.col(0)(0), 21.0);
  EXPECT_EQ(imu_measurements.col(1)(0), 25.0);
  EXPECT_EQ(imu_measurements.col(2)(0), 29.0);
}

TEST(ImuMeasurementBuffer, DeathOnAddDataNotIncreasingTimestamp) {
  vio_common::ImuMeasurementBuffer buffer(-1);

  Eigen::Matrix<double, 6, 1> imu_measurement;
  imu_measurement.setRandom();

  buffer.addMeasurement(0u, imu_measurement);
  buffer.addMeasurement(10u, imu_measurement);
  EXPECT_DEATH(buffer.addMeasurement(9u, imu_measurement), "^");
}

TEST(ImuMeasurementBuffer, TestAddMeasurements) {
  const size_t kNumMeasurements = 10;
  vio_common::ImuMeasurementBuffer buffer(-1);

  // Create IMU measurements and fill buffer.
  Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps_groundtruth(
      1, kNumMeasurements);
  Eigen::Matrix<double, 6, Eigen::Dynamic> imu_measurements_groundtruth(
      6, kNumMeasurements);

  for (size_t idx = 0; idx < kNumMeasurements; ++idx) {
    int64_t timestamp = static_cast<int64_t>(idx * 10);
    Eigen::Matrix<double, 6, 1> imu_measurement;
    imu_measurement.setConstant(idx);
    imu_timestamps_groundtruth(idx) = timestamp;
    imu_measurements_groundtruth.col(idx) = imu_measurement;
  }
  buffer.addMeasurements(
      imu_timestamps_groundtruth, imu_measurements_groundtruth);
}

MAPLAB_UNITTEST_ENTRYPOINT


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

#include "SurgSim/Collision/OctreeContact.h"

#include <boost/functional/hash.hpp>

#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/Math/BoxShape.h"
#include "SurgSim/Math/Shape.h"
#include "SurgSim/Math/Vector.h"

namespace SurgSim
{
namespace Collision
{

size_t OctreeContact::Vector3dHash::operator()(const SurgSim::Math::Vector3d& id) const
{
	return boost::hash_range(id.data(), id.data() + 3);
}

std::list<std::shared_ptr<Contact>> OctreeContact::doCalculateDcdContact(
	const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
	const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2)
{
	SURGSIM_ASSERT(posedShape1.getShape()->getType() == Math::SHAPE_TYPE_OCTREE) <<
		"Octree Contact needs an OctreeShape.";

	SurgSim::DataStructures::OctreePath nodePath;
	std::list<std::shared_ptr<Contact>> result;
	std::shared_ptr<Math::OctreeShape> shape = std::static_pointer_cast<Math::OctreeShape>(posedShape1.getShape());

	calculateContactWithNode(shape->getOctree(), posedShape1.getPose(),
		posedShape2.getShape(), posedShape2.getPose(), &nodePath, &result);

	return result;
}

void OctreeContact::calculateContactWithNode(
	std::shared_ptr<const Math::OctreeShape::NodeType> node,
	Math::RigidTransform3d octreePose,
	const std::shared_ptr<Math::Shape>& shape,
	const Math::RigidTransform3d& shapePose,
	SurgSim::DataStructures::OctreePath* nodePath,
	std::list<std::shared_ptr<Contact>>* result)
{
	if (! node->isActive())
	{
		return;
	}

	auto nodeBoundingBox = Math::transformAabb(octreePose, node->getBoundingBox());
	auto shapeBoundingBox = shape->getBoundingBox();
	if (!shape->isTransformable())
	{
		shapeBoundingBox = Math::transformAabb(shapePose, shapeBoundingBox);
	}
	if (!shapeBoundingBox.isEmpty() && !Math::doAabbIntersect(nodeBoundingBox, shapeBoundingBox))
	{
		return;
	}

	Math::Vector3d nodeSize = node->getBoundingBox().sizes();
	std::shared_ptr<Math::BoxShape> nodeShape;
	if (m_shapes.count(nodeSize) > 0)
	{
		nodeShape = m_shapes[nodeSize];
	}
	else
	{
		nodeShape = std::make_shared<SurgSim::Math::BoxShape>(nodeSize.x(), nodeSize.y(), nodeSize.z());
		m_shapes[nodeSize] = nodeShape;
	}

	Math::Vector3d nodeCenter = node->getBoundingBox().center();
	Math::RigidTransform3d nodePose = octreePose;
	nodePose.translation() += nodePose.linear() * nodeCenter;

	if (node->hasChildren())
	{
		if ((!shapeBoundingBox.isEmpty() && nodeBoundingBox.contains(shapeBoundingBox)) ||
			 !boxContactCalculation(*nodeShape, nodePose, *shape, shapePose).empty())
		{
			for (size_t i = 0; i < node->getChildren().size(); i++)
			{
				nodePath->push_back(i);
				calculateContactWithNode(node->getChild(i), octreePose, shape, shapePose, nodePath, result);
				nodePath->pop_back();
			}
		}
	}
	else
	{
		auto contacts = boxContactCalculation(*nodeShape, nodePose, *shape, shapePose);
		for (auto& contact : contacts)
		{
			contact->penetrationPoints.first.octreeNodePath.setValue(*nodePath);

			Math::Vector3d contactPosition = contact->penetrationPoints.first.rigidLocalPosition.getValue();
			contactPosition += nodeCenter;
			contact->penetrationPoints.first.rigidLocalPosition.setValue(contactPosition);
		}
		result->splice(result->end(), contacts);
	}
}

};
};


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

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

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

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

  BOOST_DISPATCH_OVERLOAD ( sum_
                          , (typename A0)
                          , bs::sse3_
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE float operator() ( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      // Use hdup to be faster than shuffle in SSE2
      __m128  bf   = _mm_movehdup_ps(a0);
      __m128  sums = _mm_add_ps(a0, bf);
      return _mm_cvtss_f32(_mm_add_ss(sums, _mm_movehl_ps(bf, sums)));
    }
  };
} } }

#endif


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

#define BOOST_TEST_MAIN

#include <string>
#include <thread>

#include <limits>

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

#include "Tudat/Basics/testMacros.h"
#include "Tudat/SimulationSetup/tudatEstimationHeader.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/desaturationDeltaV.h"

namespace tudat
{
namespace unit_tests
{
BOOST_AUTO_TEST_SUITE( test_desaturation_deltaV_values_estimation )

//Using declarations.
using namespace tudat::observation_models;
using namespace tudat::orbit_determination;
using namespace tudat::estimatable_parameters;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::orbital_element_conversions;
using namespace tudat::ephemerides;
using namespace tudat::propagators;
using namespace tudat::basic_astrodynamics;

//! Unit test to check if desaturation deltaV values are estimated correctly
BOOST_AUTO_TEST_CASE( test_DesaturationDeltaVsEstimation )
{

    // Load spice kernels.
        spice_interface::loadStandardSpiceKernels( );

        // Define bodies in simulation
        std::vector< std::string > bodyNames;
        bodyNames.push_back( "Earth" );
        bodyNames.push_back( "Sun" );
        bodyNames.push_back( "Moon" );
        bodyNames.push_back( "Mars" );

        // Specify number of observation days.
        int numberOfDaysOfData = 1;

        // Specify initial time
        double initialEphemerisTime = 1.0e7;
        double finalEphemerisTime = initialEphemerisTime + numberOfDaysOfData * 86400.0;

        // Define times and deltaV magnitudes for momentum wheel desaturation maneuvers.
        std::vector< double > thrustMidTimes = { initialEphemerisTime + 1.0 * 3600.0, initialEphemerisTime + 3.0 * 3600.0,
                                                 initialEphemerisTime + 5.0 * 3600.0 };
        std::vector< Eigen::Vector3d > deltaVValues =
        { 1.0E-3 * ( Eigen::Vector3d( ) << 0.3, -2.5, 3.4 ).finished( ),
          1.0E-3 * ( Eigen::Vector3d( ) << 2.0, 5.9, -0.5 ).finished( ),
          1.0E-3 * ( Eigen::Vector3d( ) << -1.6, 4.4, -5.8 ).finished( ) };
        double totalManeuverTime = 90.0;
        double maneuverRiseTime = 15.0;

        // Create bodies needed in simulation
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
                getDefaultBodySettings( bodyNames );
        bodySettings[ "Earth" ]->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
                    "ECLIPJ2000", "IAU_Earth", spice_interface::computeRotationQuaternionBetweenFrames(
                        "ECLIPJ2000", "IAU_Earth", initialEphemerisTime ),
                        initialEphemerisTime, 2.0 * mathematical_constants::PI / ( physical_constants::JULIAN_DAY ) );

        NamedBodyMap bodyMap = createBodies( bodySettings );
        bodyMap[ "Vehicle" ] = std::make_shared< Body >( );
        bodyMap[ "Vehicle" ]->setConstantBodyMass( 400.0 );

        // Create aerodynamic coefficient interface settings.
        double referenceArea = 4.0;
        double aerodynamicCoefficient = 1.2;
        std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings =
                std::make_shared< ConstantAerodynamicCoefficientSettings >(
                    referenceArea, aerodynamicCoefficient * ( Eigen::Vector3d( ) << 1.2, -0.1, -0.4 ).finished( ), 1, 1 );

        // Create and set aerodynamic coefficients object
        bodyMap[ "Vehicle" ]->setAerodynamicCoefficientInterface(
                    createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "Vehicle" ) );

        // Create radiation pressure settings
        double referenceAreaRadiation = 4.0;
        double radiationPressureCoefficient = 1.2;
        std::vector< std::string > occultingBodies;
        occultingBodies.push_back( "Earth" );
        std::shared_ptr< RadiationPressureInterfaceSettings > asterixRadiationPressureSettings =
                std::make_shared< CannonBallRadiationPressureInterfaceSettings >(
                    "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies );

        // Create and set radiation pressure settings
        bodyMap[ "Vehicle" ]->setRadiationPressureInterface(
                    "Sun", createRadiationPressureInterface(
                        asterixRadiationPressureSettings, "Vehicle", bodyMap ) );

        bodyMap[ "Vehicle" ]->setEphemeris( std::make_shared< TabulatedCartesianEphemeris< > >(
                                                std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                < double, Eigen::Vector6d > >( ), "Earth", "ECLIPJ2000" ) );

        setGlobalFrameBodyEphemerides( bodyMap, "Earth", "ECLIPJ2000" );

        // Create ground stations.
        std::vector< std::string > groundStationNames;
        groundStationNames.push_back( "Station1" );
        groundStationNames.push_back( "Station2" );
        groundStationNames.push_back( "Station3" );

        createGroundStation( bodyMap.at( "Earth" ), "Station1", ( Eigen::Vector3d( ) << 0.0, 0.35, 0.0 ).finished( ),
                             coordinate_conversions::geodetic_position );
        createGroundStation( bodyMap.at( "Earth" ), "Station2", ( Eigen::Vector3d( ) << 0.0, -0.55, 2.0 ).finished( ),
                             coordinate_conversions::geodetic_position );
        createGroundStation( bodyMap.at( "Earth" ), "Station3", ( Eigen::Vector3d( ) << 0.0, 0.05, 4.0 ).finished( ),
                             coordinate_conversions::geodetic_position );

        // Set accelerations on Vehicle that are to be taken into account.
        SelectedAccelerationMap accelerationMap;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;
        accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 8, 8 ) );
        accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                       basic_astrodynamics::central_gravity ) );
        accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                        basic_astrodynamics::central_gravity ) );
        accelerationsOfVehicle[ "Mars" ].push_back( std::make_shared< AccelerationSettings >(
                                                        basic_astrodynamics::central_gravity ) );
        accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                       basic_astrodynamics::cannon_ball_radiation_pressure ) );
        accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                         basic_astrodynamics::aerodynamic ) );
        accelerationsOfVehicle[ "Vehicle" ].push_back( std::make_shared< MomentumWheelDesaturationAccelerationSettings >(
                        thrustMidTimes, deltaVValues, totalManeuverTime, maneuverRiseTime ) );

        accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;

        // Set bodies for which initial state is to be estimated and integrated.
        std::vector< std::string > bodiesToIntegrate;
        std::vector< std::string > centralBodies;
        bodiesToIntegrate.push_back( "Vehicle" );
        centralBodies.push_back( "Earth" );

        // Create acceleration models
        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToIntegrate, centralBodies );

        // Set Keplerian elements for Vehicle.
        Eigen::Vector6d initialStateInKeplerianElements;
        initialStateInKeplerianElements( semiMajorAxisIndex ) = 7200.0E3;
        initialStateInKeplerianElements( eccentricityIndex ) = 0.05;
        initialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
        initialStateInKeplerianElements( argumentOfPeriapsisIndex )
                = unit_conversions::convertDegreesToRadians( 235.7 );
        initialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                = unit_conversions::convertDegreesToRadians( 23.4 );
        initialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

        double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );

        // Set (perturbed) initial state.
        Eigen::Matrix< double, 6, 1 > systemInitialState = convertKeplerianToCartesianElements(
                    initialStateInKeplerianElements, earthGravitationalParameter );

        // Create propagator settings.
        std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, finalEphemerisTime, cowell );

        // Create integrator settings.
        std::shared_ptr< IntegratorSettings< double > > integratorSettings
                = std::make_shared< RungeKuttaVariableStepSizeSettings< double > >
                ( initialEphemerisTime, 40.0, RungeKuttaCoefficients::CoefficientSets::rungeKuttaFehlberg78,
                  40.0, 40.0, 1.0, 1.0 );

        // Define link ends.
        std::vector< LinkEnds > stationReceiverLinkEnds;
        std::vector< LinkEnds > stationTransmitterLinkEnds;

        for( unsigned int i = 0; i < groundStationNames.size( ); i++ )
        {
            LinkEnds linkEnds;
            linkEnds[ transmitter ] = std::make_pair( "Earth", groundStationNames.at( i ) );
            linkEnds[ receiver ] = std::make_pair( "Vehicle", "" );
            stationTransmitterLinkEnds.push_back( linkEnds );

            linkEnds.clear( );
            linkEnds[ receiver ] = std::make_pair( "Earth", groundStationNames.at( i ) );
            linkEnds[ transmitter ] = std::make_pair( "Vehicle", "" );
            stationReceiverLinkEnds.push_back( linkEnds );
        }

        std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable;
        linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 0 ] );
        linkEndsPerObservable[ one_way_range ].push_back( stationTransmitterLinkEnds[ 0 ] );
        linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 1 ] );

        linkEndsPerObservable[ one_way_doppler ].push_back( stationReceiverLinkEnds[ 1 ] );
        linkEndsPerObservable[ one_way_doppler ].push_back( stationTransmitterLinkEnds[ 2 ] );

        linkEndsPerObservable[ angular_position ].push_back( stationReceiverLinkEnds[ 2 ] );
        linkEndsPerObservable[ angular_position ].push_back( stationTransmitterLinkEnds[ 1 ] );

        // Define parameters to be estimated.
        std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
        parameterNames.push_back( std::make_shared< InitialTranslationalStateEstimatableParameterSettings< double > >(
                        "Vehicle", systemInitialState, "Earth" ) );

        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Vehicle", desaturation_delta_v_values ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Vehicle", radiation_pressure_coefficient ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Vehicle", constant_drag_coefficient ) );

        parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                      2, 0, 2, 2, "Earth", spherical_harmonics_cosine_coefficient_block ) );
        parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                      2, 1, 2, 2, "Earth", spherical_harmonics_sine_coefficient_block ) );

        parameterNames.push_back(  std::make_shared< EstimatableParameterSettings > ( "Earth", rotation_pole_position ) );
        parameterNames.push_back(  std::make_shared< EstimatableParameterSettings > ( "Earth", ground_station_position, "Station1" ) );

        // Create parameters
        std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
                createParametersToEstimate< double >( parameterNames, bodyMap , propagatorSettings );

        printEstimatableParameterEntries( parametersToEstimate );

        observation_models::ObservationSettingsMap observationSettingsMap;
        for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
             linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
        {
            ObservableType currentObservable = linkEndIterator->first;

            std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;
            for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
            {
                observationSettingsMap.insert( std::make_pair( currentLinkEndsList.at( i ), std::make_shared< ObservationSettings >
                                                               ( currentObservable, std::shared_ptr< LightTimeCorrectionSettings >( ) ) ) );
            }
        }

        // Create orbit determination object.
        OrbitDeterminationManager< double, double > orbitDeterminationManager = OrbitDeterminationManager< double, double >(
                    bodyMap, parametersToEstimate, observationSettingsMap, integratorSettings, propagatorSettings );

        // Compute list of observation times.
        std::vector< double > baseTimeList;
        double observationTimeStart = initialEphemerisTime + 1000.0;
        double  observationInterval = 60.0;
        for( int i = 0; i < numberOfDaysOfData; i++ )
        {
            for( unsigned int j = 0; j < 500; j++ )
            {
                baseTimeList.push_back( observationTimeStart + static_cast< double >( i ) * 86400.0 +
                                        static_cast< double >( j ) * observationInterval );
            }
        }

        std::map< ObservableType, std::map< LinkEnds, std::pair< std::vector< double >, LinkEndType > > > measurementSimulationInput;
        for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
             linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
        {
            ObservableType currentObservable = linkEndIterator->first;
            std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;
            for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
            {
                measurementSimulationInput[ currentObservable ][ currentLinkEndsList.at( i ) ] =
                        std::make_pair( baseTimeList, receiver );
            }
        }

        typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > ObservationVectorType;
        typedef std::map< LinkEnds, std::pair< ObservationVectorType, std::pair< std::vector< double >, LinkEndType > > > SingleObservablePodInputType;
        typedef std::map< ObservableType, SingleObservablePodInputType > PodInputDataType;

        // Simulate observations.
        PodInputDataType observationsAndTimes = simulateObservations< double, double >(
                    measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ) );

        // Perturb parameter estimate.
        Eigen::Matrix< double, Eigen::Dynamic, 1 > initialParameterEstimate =
                parametersToEstimate->template getFullParameterValues< double >( );
        Eigen::Matrix< double, Eigen::Dynamic, 1 > truthParameters = initialParameterEstimate;
        Eigen::Matrix< double, Eigen::Dynamic, 1 > parameterPerturbation =
                Eigen::Matrix< double, Eigen::Dynamic, 1 >::Zero( truthParameters.rows( ) );

        // Perturbe initial state estimate.
        parameterPerturbation.segment( 0, 3 ) = Eigen::Vector3d::Constant( 1.0 );
        parameterPerturbation.segment( 3, 3 ) = Eigen::Vector3d::Constant( 1.E-3 );

        // Perturb deltaVs estimate.
        for ( unsigned int i = 8 ; i < 8 + deltaVValues.size() * 3 ; i++)
        {
            parameterPerturbation[ i ] = 1.0e-3;
        }

        initialParameterEstimate += parameterPerturbation;

        // Define estimation input
        std::shared_ptr< PodInput< double, double  > > podInput = std::make_shared< PodInput< double, double > >(
                    observationsAndTimes, initialParameterEstimate.rows( ),
                    Eigen::MatrixXd::Zero( truthParameters.rows( ), truthParameters.rows( ) ),
                    initialParameterEstimate - truthParameters );

        std::map< observation_models::ObservableType, double > weightPerObservable;
        weightPerObservable[ one_way_range ] = 1.0 / ( 1.0 * 1.0 );
        weightPerObservable[ angular_position ] = 1.0 / ( 1.0E-5 * 1.0E-5 );
        weightPerObservable[ one_way_doppler ] = 1.0 / ( 1.0E-11 * 1.0E-11 );

        podInput->setConstantPerObservableWeightsMatrix( weightPerObservable );
        podInput->defineEstimationSettings( true, true, true, true, false );

        // Perform estimation
        std::shared_ptr< PodOutput< double > > podOutput = orbitDeterminationManager.estimateParameters(
                    podInput, std::make_shared< EstimationConvergenceChecker >( 4 ) );

        Eigen::VectorXd estimationError = podOutput->parameterEstimate_ - truthParameters;
        std::cout <<"Estimation error: "<< ( estimationError ).transpose( ) << std::endl;

        // Check if parameters are correctly estimated
        Eigen::VectorXd estimatedParametervalues = podOutput->parameterEstimate_;

        // Initial state.
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i ) - podOutput->parameterEstimate_( i ) ), 0.1 );
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i + 3 ) - podOutput->parameterEstimate_( i + 3 ) ), 1.0E-6 );
        }
        // Radiation pressure and drag coefficients.
        BOOST_CHECK_SMALL( std::fabs( truthParameters( 6 ) - podOutput->parameterEstimate_( 6 ) ), 1.0e-4 );
        BOOST_CHECK_SMALL( std::fabs( truthParameters( 7 ) - podOutput->parameterEstimate_( 7 ) ), 1.0e-4 );

        // Momentum wheel desaturation deltaV values.
        for ( unsigned int i = 8 ; i < 8 + deltaVValues.size() * 3 ; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i ) - podOutput->parameterEstimate_( i ) ), 1.0E-10 );
        }

        // Gravity field coefficients.
        for ( unsigned int i = 17 ; i < 22 ; i++ ){
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i ) - podOutput->parameterEstimate_( i ) ), 1.0E-12 );
        }

        // Earth pole position.
        for ( unsigned int i = 22 ; i < 24 ; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i ) - podOutput->parameterEstimate_( i ) ), 1.0E-12 );
        }

        // Ground station position.
        for ( unsigned int i = 24 ; i < 27 ; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i ) - podOutput->parameterEstimate_( i ) ), 1.0E-6 );
        }

    }

}

BOOST_AUTO_TEST_SUITE_END( )

}


#include <base/cgal_typedefs.h>

#include <IO/fileIO.h>
#include <IO/ttIO.h>
#include <IO/ethIO.h>
#include <util/vectorArithmetic.h>
#include <util/helper.h>
#include <learning/learning.h>
#include <IO/meshlab_project.h>
#include <rPLY/ply.h>
#include <processing/pointSetProcessing.h>

#include <Eigen/Core>
#include <boost/filesystem.hpp>

#include <pcl/console/parse.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h>

#include "IO/meshlab_project.h"
#include "IO/util.h"

using namespace std;

////////////////////////////////////////////////////////////
/////////////////////// ETH3D I/O ///////////////////////////
////////////////////////////////////////////////////////////

int importETH3D(dirHolder dir, dataHolder& data){

    cout << "\nLoad ETH3D meshlab project..." << endl;

    pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);

    // Load the ground truth point cloud poses from the MeshLab project file.
    MeshLabMeshInfoVector scan_infos;

    string ground_truth_mlp_path = dir.path + "scan_clean/scan_alignment.mlp";

    if (!ReadMeshLabProject(ground_truth_mlp_path, &scan_infos)){
      std::cerr << "Cannot read scan poses from " << ground_truth_mlp_path
                << std::endl;
      return 1;
    }

    // Validate the scan transformations (partly: not checking that the top-left
    // 3x3 block is a rotation matrix).
    for (size_t scan_index = 0; scan_index < scan_infos.size(); ++scan_index) {
      const MeshLabProjectMeshInfo& info = scan_infos[scan_index];
      if (info.global_T_mesh(3, 0) != 0 || info.global_T_mesh(3, 1) != 0 ||
          info.global_T_mesh(3, 2) != 0 || info.global_T_mesh(3, 3) != 1) {
        std::cerr << "Error: Last row in a scan's transformation matrix is not"
                     " (0, 0, 0, 1)."
                  << std::endl;
        return 1;
      }
    }

    // Load the ground truth scan point clouds.
    std::vector<PointCloudPtr> scans;
    for (const MeshLabProjectMeshInfo& scan_info : scan_infos) {
      // Get absolute or compose relative path.
      std::string file_path =
          (scan_info.filename.empty() || scan_info.filename[0] == '/')
              ? scan_info.filename
              : (boost::filesystem::path(ground_truth_mlp_path).parent_path() /
                 scan_info.filename)
                    .string();

      std::cout << "\t-loading scan: " << scan_info.filename << std::endl;
      PointCloudPtr point_cloud(new PointCloud());
      if (pcl::io::loadPLYFile(file_path, *point_cloud) < 0) {
        std::cerr << "Cannot read scan file." << std::endl;
        return 1;
      }

      scans.push_back(point_cloud);
    }

//    PointCloudPtr scan(new PointCloud());
    for (size_t scan_index = 0; scan_index < scan_infos.size(); ++scan_index){
        PointCloud temp_cloud;
        pcl::transformPointCloud(*scans[scan_index], temp_cloud,
                               scan_infos[scan_index].global_T_mesh);
        for (const auto & pt : temp_cloud.points){
            Point p(pt.x, pt.y, pt.z );
            if(!pointInsideBbox(p,data.sm_obb))
                continue;
            data.points.push_back(p);
            Point s(scan_infos[scan_index].global_T_mesh(0,3),
                  scan_infos[scan_index].global_T_mesh(1,3),
                  scan_infos[scan_index].global_T_mesh(2,3));
            vertex_info vinf;
            vinf.sensor_positions.push_back(s);
            vinf.sensor_vec = s - p;
            vinf.sensor_vec = vinf.sensor_vec/std::sqrt(vinf.sensor_vec.squared_length());
            data.infos.push_back(vinf);
        }
    }

    data.has_sensor = true;
    data.has_normal = false;

    return 0;
}

//int pcl2cgal(dataHolder& data, PointCloudPtr& cloud)
//{

//    for (long long int scan_point_index = 0; scan_point_index < scan_point_size;
//         ++scan_point_index) {
//      const pcl::PointXYZ& scan_point = scan->at(scan_point_index);

//    vertex_info vinf;
//    for (const auto & pt : *cloud.points) {
//    Point p (pt.x, pt.y, pt.z );
//    data.points.push_back(p);
//    Vector n (pt.normal_x, pt.normal_y, pt.normal_z);
//    vinf.normal = n;
//    vinf.sensor_positions[0] = p;
//    data.infos.push_back(vinf);
//    }

//    return 0;
//}


/** $Id: StaticSurfaceInjector.cxx 136127 2015-08-12 13:54:32Z jvansanten $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 136127 $
 * $Date: 2015-08-12 07:54:32 -0600 (Wed, 12 Aug 2015) $
 */

#include <MuonGun/I3MuonGun.h>
#include <MuonGun/StaticSurfaceInjector.h>
#include <MuonGun/Cylinder.h>
#include <dataclasses/I3Constants.h>

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>

#include <icetray/I3Module.h>
#include <dataclasses/I3Double.h>
#include <dataclasses/physics/I3MCTree.h>
#include <dataclasses/physics/I3MCTreeUtils.h>
#include <phys-services/I3RandomService.h>

namespace I3MuonGun {

namespace {

std::string
GetTablePath(const std::string &subpath)
{
	std::ostringstream tablePath;
	tablePath << getenv("I3_BUILD") << "/MuonGun/resources/tables/" << subpath;
	return tablePath.str();
}

}

template <typename Archive>
void
StaticSurfaceInjector::serialize(Archive &ar, unsigned version)
{
	if (version > 1)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("Generator", base_object<Generator>(*this));
	if (version == 0) {
		CylinderPtr cyl;
		ar & make_nvp("Surface", cyl);
		surface_ = cyl;
	} else {
		ar & make_nvp("Surface", surface_);
	}
	ar & make_nvp("Flux", flux_);
	ar & make_nvp("EnergySpectrum", energyGenerator_);
	ar & make_nvp("RadialDistribution", radialDistribution_);
	ar & make_nvp("MaxFlux", maxFlux_);
	ar & make_nvp("TotalRate", totalRate_);
}

StaticSurfaceInjector::StaticSurfaceInjector()
{
	SetSurface(boost::make_shared<Cylinder>(1600, 800));

	FluxPtr flux = boost::make_shared<SplineFlux>(
	    GetTablePath("Hoerandel5_atmod12_SIBYLL.single_flux.fits"),
	    GetTablePath("Hoerandel5_atmod12_SIBYLL.bundle_flux.fits"));
	flux->SetMinMultiplicity(1);
	flux->SetMaxMultiplicity(1);
	SetFlux(flux);

	energyGenerator_ = boost::make_shared<OffsetPowerLaw>(2, 500., 50, 1e6);

	radialDistribution_ = boost::make_shared<SplineRadialDistribution>(
	    GetTablePath("Hoerandel5_atmod12_SIBYLL.radius.fits"));
}

StaticSurfaceInjector::StaticSurfaceInjector(SamplingSurfacePtr surface, FluxPtr flux,
    boost::shared_ptr<OffsetPowerLaw> edist, RadialDistributionPtr rdist)
{
	SetSurface(surface);
	SetFlux(flux);
	energyGenerator_ = edist;
	radialDistribution_ = rdist;
}

GenerationProbabilityPtr
StaticSurfaceInjector::Clone() const
{
	return boost::make_shared<StaticSurfaceInjector>(*this);
}

bool
StaticSurfaceInjector::IsCompatible(GenerationProbabilityConstPtr o) const
{
	boost::shared_ptr<const StaticSurfaceInjector> other
	    = boost::dynamic_pointer_cast<const StaticSurfaceInjector>(o);
	if (!other)
		return false;
	return (*surface_ == *(other->surface_) && *flux_ == *(other->flux_)
	    && *radialDistribution_ == *(other->radialDistribution_)
	    && *energyGenerator_ == *(other->energyGenerator_));
}

void
StaticSurfaceInjector::SetSurface(SamplingSurfacePtr p)
{
	surface_ = p;
	totalRate_ = NAN;
	zenithNorm_ = NAN;
	CalculateMaxFlux();
}

void
StaticSurfaceInjector::SetFlux(FluxPtr p)
{
	flux_ = p;
	totalRate_ = NAN;
	zenithNorm_ = NAN;
	CalculateMaxFlux();
}

void
StaticSurfaceInjector::CalculateMaxFlux()
{
	if (surface_ && flux_)
		maxFlux_ = (*flux_)(surface_->GetMinDepth(), 1., 1u)*surface_->GetMaximumArea();
}

double
StaticSurfaceInjector::GetTotalRate() const
{
	if (std::isnan(totalRate_) && surface_ && flux_) {
		totalRate_ = 0;
		for (unsigned m = flux_->GetMinMultiplicity(); m <= flux_->GetMaxMultiplicity(); m++) {
			totalRate_ += surface_->IntegrateFlux(boost::bind(boost::cref(*flux_), _1, _2, m));
		}
	}
	return totalRate_;
}

double
StaticSurfaceInjector::GetZenithNorm() const
{
	if (std::isnan(zenithNorm_) && surface_ && flux_) {
		zenithNorm_ = 0;
		for (unsigned m = flux_->GetMinMultiplicity(); m <= flux_->GetMaxMultiplicity(); m++) {
			zenithNorm_ += Integrate(boost::bind(boost::cref(*flux_), surface_->GetMinDepth(), _1, m), 0, 1);
		}
		zenithNorm_ = std::log(zenithNorm_);
	}
	return zenithNorm_;
}

void
StaticSurfaceInjector::GenerateAxis(I3RandomService &rng, std::pair<I3Particle, unsigned> &axis) const
{
	// Choose a direction and impact position from a uniform flux through
	// the sampling surface, then pick a multiplicity. Accept zenith angles
	// and multiplicities at a rate proportional to the flux at the shallowest depth.
	I3Direction dir;
	I3Position pos;
	unsigned m;
	double flux;
	// For any sane distribution the maximum flux is for single vertical muons
	// at minimum depth
	double maxflux = (*flux_)(surface_->GetMinDepth(), 1., flux_->GetMinMultiplicity());
	do {
		surface_->SampleImpactRay(pos, dir, rng);
		m = rng.Integer(flux_->GetMaxMultiplicity() - flux_->GetMinMultiplicity())
		    + flux_->GetMinMultiplicity();
		flux = (*flux_)(surface_->GetMinDepth(), cos(dir.GetZenith()), m);
	} while (rng.Uniform(0., maxflux) > flux);

	axis.first.SetPos(pos);
	axis.first.SetDir(dir);
	axis.first.SetShape(I3Particle::Primary);
	axis.first.SetLocationType(I3Particle::Anywhere);
	axis.first.SetType(I3Particle::unknown);
	axis.first.SetTime(0.);
	axis.second = m;
}

void
StaticSurfaceInjector::FillMCTree(I3RandomService &rng,
    const std::pair<I3Particle, unsigned> &axis,
    I3MCTree &mctree, BundleConfiguration &bundlespec) const
{
	const I3Particle &primary = axis.first;
	I3MCTreeUtils::AddPrimary(mctree, primary);
	double h = GetDepth(primary.GetPos().GetZ());
	double coszen = cos(primary.GetDir().GetZenith());

	unsigned m = axis.second;
	for (unsigned i=0; i < m; i++) {
		double radius = 0., azimuth = 0.;
		if (m > 1u) {
			radius = radialDistribution_->Generate(rng, h, coszen, m);
			azimuth = rng.Uniform(0., 2*M_PI);
		}

		I3Particle track = CreateParallelTrack(radius, azimuth, *surface_, primary);

		track.SetEnergy(energyGenerator_->Generate(rng));
		I3MCTreeUtils::AppendChild(mctree, primary, track);
		bundlespec.push_back(BundleEntry(radius, track.GetEnergy()));
	}
}

void
StaticSurfaceInjector::Generate(I3RandomService &rng, I3MCTree &mctree, BundleConfiguration &bundlespec) const
{
	std::pair<I3Particle, unsigned> axis;
	GenerateAxis(rng, axis);
	FillMCTree(rng, axis, mctree, bundlespec);
}

double
StaticSurfaceInjector::GetLogGenerationProbability(const I3Particle &axis,
    const BundleConfiguration &bundlespec) const
{
	std::pair<double, double> steps = surface_->GetIntersection(axis.GetPos(), axis.GetDir());
	// This shower axis doesn't intersect the sampling surface. Bail.
	if (!std::isfinite(steps.first))
		return -std::numeric_limits<double>::infinity();

	double h = GetDepth(axis.GetPos().GetZ() + steps.first*axis.GetDir().GetZ());
	double coszen = cos(axis.GetDir().GetZenith());
	unsigned m = static_cast<unsigned>(bundlespec.size());

	// We used the flux to do rejection sampling in zenith and multiplicity. Evaluate
	// the properly-normalized PDF here.
	double logprob = flux_->GetLog(surface_->GetMinDepth(), coszen, m) - GetZenithNorm();
	BOOST_FOREACH(const BundleEntry &track, bundlespec) {
		if (m > 1)
			logprob += radialDistribution_->GetLog(h, coszen, m, track.radius);
		logprob += energyGenerator_->GetLog(track.energy);
	}

	return logprob - std::log(surface_->GetAcceptance());
}

}

I3_SERIALIZABLE(I3MuonGun::StaticSurfaceInjector);


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

#include <algorithm>
#include <boost/asio.hpp>
#include <functional>
#include <iostream>
#include <memory>
#include <numeric>
#include <string>
#include <vector>

#include "ngraph/log.hpp"
#include "seal/he_seal_client.hpp"
#include "seal/seal.h"
#include "seal/seal_util.hpp"
#include "tcp/tcp_client.hpp"
#include "tcp/tcp_message.hpp"

ngraph::he::HESealClient::HESealClient(const std::string& hostname,
                                       const size_t port,
                                       const size_t batch_size,
                                       const std::vector<float>& inputs)
    : m_batch_size{batch_size}, m_is_done(false), m_inputs{inputs} {
  boost::asio::io_context io_context;
  tcp::resolver resolver(io_context);
  auto endpoints = resolver.resolve(hostname, std::to_string(port));

  auto client_callback = [this](const ngraph::he::TCPMessage& message) {
    return handle_message(message);
  };

  m_tcp_client = std::make_shared<ngraph::he::TCPClient>(io_context, endpoints,
                                                         client_callback);

  io_context.run();
}

void ngraph::he::HESealClient::set_seal_context() {
  m_context = seal::SEALContext::Create(m_encryption_params, true,
                                        seal::sec_level_type::none);

  print_seal_context(*m_context);

  m_keygen = std::make_shared<seal::KeyGenerator>(m_context);
  m_relin_keys = std::make_shared<seal::RelinKeys>(m_keygen->relin_keys());
  m_public_key = std::make_shared<seal::PublicKey>(m_keygen->public_key());
  m_secret_key = std::make_shared<seal::SecretKey>(m_keygen->secret_key());
  m_encryptor = std::make_shared<seal::Encryptor>(m_context, *m_public_key);
  m_decryptor = std::make_shared<seal::Decryptor>(m_context, *m_secret_key);

  // Evaluator
  m_evaluator = std::make_shared<seal::Evaluator>(m_context);

  // Encoder
  m_ckks_encoder = std::make_shared<seal::CKKSEncoder>(m_context);

  // TODO: pick better scale?
  m_scale = ngraph::he::choose_scale(m_encryption_params.coeff_modulus());
  NGRAPH_INFO << "Client scale " << m_scale;
}

void ngraph::he::HESealClient::handle_message(
    const ngraph::he::TCPMessage& message) {
  ngraph::he::MessageType msg_type = message.message_type();

  // NGRAPH_INFO << "Client received message type: "
  //          << message_type_to_string(msg_type).c_str() ;

  switch (msg_type) {
    case ngraph::he::MessageType::parameter_size: {
      // Number of (packed) ciphertexts to perform inference on
      size_t parameter_size;
      std::memcpy(&parameter_size, message.data_ptr(), message.data_size());

      const size_t complex_pack_factor = complex_packing() ? 2 : 1;

      NGRAPH_INFO << "Parameter size " << parameter_size;
      NGRAPH_INFO << "Client batch size " << m_batch_size;
      if (complex_packing()) {
        NGRAPH_INFO << "Client complex packing";
        // TODO: support odd batch sizes
        assert(m_batch_size % 2 == 0);
      }

      // TODO: allow smaller sizes!
      if (m_inputs.size() !=
          parameter_size * m_batch_size * complex_pack_factor) {
        NGRAPH_INFO << "m_inputs.size() " << m_inputs.size()
                    << " != paramter_size ( " << parameter_size
                    << ") * m_batch_size (" << m_batch_size
                    << ") * complex_pack_factor (" << complex_pack_factor
                    << ")";
      }
      NGRAPH_CHECK(m_inputs.size() ==
                       parameter_size * m_batch_size * complex_pack_factor,
                   "m_inputs.size()", m_inputs.size(), "parameter_size",
                   parameter_size, "m_batch_size", m_batch_size,
                   "complex_pack_factor", complex_pack_factor);

      std::vector<seal::Ciphertext> ciphers(parameter_size);
#pragma omp parallel for
      for (size_t data_idx = 0; data_idx < parameter_size; ++data_idx) {
        seal::Plaintext plain;

        size_t batch_start_idx = data_idx * m_batch_size * complex_pack_factor;
        size_t batch_end_idx =
            batch_start_idx + m_batch_size * complex_pack_factor;

        std::vector<double> real_vals{m_inputs.begin() + batch_start_idx,
                                      m_inputs.begin() + batch_end_idx};
        if (complex_packing()) {
          std::vector<std::complex<double>> complex_vals;
          real_vec_to_complex_vec(complex_vals, real_vals);
          m_ckks_encoder->encode(complex_vals, m_scale, plain);
        } else {
          m_ckks_encoder->encode(real_vals, m_scale, plain);
        }
        m_encryptor->encrypt(plain, ciphers[data_idx]);
      }
      NGRAPH_INFO << "Creating execute message";
      auto execute_message =
          TCPMessage(ngraph::he::MessageType::execute, ciphers);
      NGRAPH_INFO << "Sending execute message with " << parameter_size
                  << " ciphertexts";
      write_message(std::move(execute_message));
      break;
    }
    case ngraph::he::MessageType::result: {
      size_t result_count = message.count();
      size_t element_size = message.element_size();

      NGRAPH_INFO << "Client got " << result_count << " results ";

      std::vector<seal::Ciphertext> result;
      m_results.reserve(result_count * m_batch_size);
      for (size_t result_idx = 0; result_idx < result_count; ++result_idx) {
        seal::Ciphertext cipher;
        std::stringstream cipher_stream;
        cipher_stream.write(message.data_ptr() + result_idx * element_size,
                            element_size);
        cipher.load(m_context, cipher_stream);

        result.push_back(cipher);
        seal::Plaintext plain;
        m_decryptor->decrypt(cipher, plain);

        std::vector<double> outputs;
        decode_to_real_vec(plain, outputs, complex_packing());
        m_results.insert(m_results.end(), outputs.begin(), outputs.end());
      }
      NGRAPH_INFO << "Results size " << m_results.size();

      close_connection();
      break;
    }

    case ngraph::he::MessageType::none: {
      close_connection();
      break;
    }

    case ngraph::he::MessageType::encryption_parameters: {
      std::stringstream param_stream;
      param_stream.write(message.data_ptr(), message.element_size());
      m_encryption_params = seal::EncryptionParameters::Load(param_stream);
      NGRAPH_INFO << "Loaded encryption parmeters";

      set_seal_context();

      // Send public key
      std::stringstream pk_stream;
      m_public_key->save(pk_stream);
      auto pk_message = TCPMessage(ngraph::he::MessageType::public_key, 1,
                                   std::move(pk_stream));
      NGRAPH_INFO << "Sending public key";
      write_message(std::move(pk_message));

      // Send evaluation key
      std::stringstream evk_stream;
      m_relin_keys->save(evk_stream);
      auto evk_message = TCPMessage(ngraph::he::MessageType::eval_key, 1,
                                    std::move(evk_stream));
      NGRAPH_INFO << "Sending evaluation key";
      write_message(std::move(evk_message));

      break;
    }
    case ngraph::he::MessageType::relu6_request: {
      handle_relu_request(message);
      break;
    }
    case ngraph::he::MessageType::relu_request: {
      handle_relu_request(message);
      break;
    }

    case ngraph::he::MessageType::max_request: {
      size_t complex_pack_factor = complex_packing() ? 2 : 1;
      size_t cipher_count = message.count();
      size_t element_size = message.element_size();

      std::vector<std::vector<double>> input_cipher_values(
          m_batch_size * complex_pack_factor,
          std::vector<double>(cipher_count, 0));

      std::vector<double> max_values(m_batch_size * complex_pack_factor,
                                     std::numeric_limits<double>::lowest());

#pragma omp parallel for
      for (size_t cipher_idx = 0; cipher_idx < cipher_count; ++cipher_idx) {
        seal::Ciphertext pre_sort_cipher;
        seal::Plaintext pre_sort_plain;

        // Load cipher from stream
        std::stringstream pre_sort_cipher_stream;
        pre_sort_cipher_stream.write(
            message.data_ptr() + cipher_idx * element_size, element_size);
        pre_sort_cipher.load(m_context, pre_sort_cipher_stream);

        // Decrypt cipher
        m_decryptor->decrypt(pre_sort_cipher, pre_sort_plain);
        std::vector<double> pre_max_value;
        decode_to_real_vec(pre_sort_plain, pre_max_value, complex_packing());

        for (size_t batch_idx = 0;
             batch_idx < m_batch_size * complex_pack_factor; ++batch_idx) {
          input_cipher_values[batch_idx][cipher_idx] = pre_max_value[batch_idx];
        }
      }

      // Get max of each vector of values
      for (size_t batch_idx = 0; batch_idx < m_batch_size * complex_pack_factor;
           ++batch_idx) {
        max_values[batch_idx] =
            *std::max_element(input_cipher_values[batch_idx].begin(),
                              input_cipher_values[batch_idx].end());
      }

      // Encrypt maximum values
      seal::Ciphertext cipher_max;
      seal::Plaintext plain_max;
      std::stringstream max_stream;

      if (complex_packing()) {
        assert(max_values.size() % 2 == 0);
        std::vector<std::complex<double>> max_complex_vals;
        real_vec_to_complex_vec(max_complex_vals, max_values);
        m_ckks_encoder->encode(max_complex_vals, m_scale, plain_max);
      } else {
        m_ckks_encoder->encode(max_values, m_scale, plain_max);
      }
      m_encryptor->encrypt(plain_max, cipher_max);
      cipher_max.save(max_stream);

      auto max_result_msg = TCPMessage(ngraph::he::MessageType::max_result, 1,
                                       std::move(max_stream));
      write_message(std::move(max_result_msg));

      break;
    }
    case ngraph::he::MessageType::execute:
    case ngraph::he::MessageType::max_result:
    case ngraph::he::MessageType::minimum_request:
    case ngraph::he::MessageType::minimum_result:
    case ngraph::he::MessageType::parameter_shape_request:
    case ngraph::he::MessageType::public_key:
    case ngraph::he::MessageType::relu_result:
    case ngraph::he::MessageType::result_request:
    default:
      NGRAPH_INFO << "Unsupported message type: "
                  << message_type_to_string(msg_type).c_str();
  }
}

void ngraph::he::HESealClient::close_connection() {
  NGRAPH_INFO << "Closing connection";
  m_tcp_client->close();
  m_is_done = true;
}

void ngraph::he::HESealClient::handle_relu_request(
    const ngraph::he::TCPMessage& message) {
  auto relu = [=](double d) { return d > 0 ? d : 0; };
  auto relu6 = [=](double d) { return d > 6.0 ? 6.0 : (d > 0) ? d : 0.; };

  std::function<double(double)> activation;

  if (message.message_type() == ngraph::he::MessageType::relu6_request) {
    activation = relu6;
  } else if (message.message_type() == ngraph::he::MessageType::relu_request) {
    activation = relu;
  } else {
    throw ngraph_error("Non-relu message type in handle_relu_request");
  }

  size_t result_count = message.count();
  size_t element_size = message.element_size();
  // NGRAPH_INFO << "Received Relu request with " << result_count << " elements"
  //            << " of size " << element_size;

  std::vector<seal::Ciphertext> post_relu_ciphers(result_count);
#pragma omp parallel for
  for (size_t result_idx = 0; result_idx < result_count; ++result_idx) {
    seal::Ciphertext pre_relu_cipher;
    seal::Plaintext relu_plain;

    // Load cipher from stream
    std::stringstream pre_relu_cipher_stream;
    pre_relu_cipher_stream.write(message.data_ptr() + result_idx * element_size,
                                 element_size);
    pre_relu_cipher.load(m_context, pre_relu_cipher_stream);

    // Decrypt cipher
    m_decryptor->decrypt(pre_relu_cipher, relu_plain);

    std::vector<double> relu_vals;
    decode_to_real_vec(relu_plain, relu_vals, complex_packing());

    std::vector<double> post_relu_vals(relu_vals.size());
    std::transform(relu_vals.begin(), relu_vals.end(), post_relu_vals.begin(),
                   activation);

    if (complex_packing()) {
      std::vector<std::complex<double>> complex_relu_vals;
      real_vec_to_complex_vec(complex_relu_vals, post_relu_vals);
      m_ckks_encoder->encode(complex_relu_vals, m_scale, relu_plain);
    } else {
      m_ckks_encoder->encode(post_relu_vals, m_scale, relu_plain);
    }
    m_encryptor->encrypt(relu_plain, post_relu_ciphers[result_idx]);
  }
  auto relu_result_msg =
      TCPMessage(ngraph::he::MessageType::relu_result, post_relu_ciphers);
  // NGRAPH_INFO << "Writing relu_result message with " << result_count
  //            << " ciphertexts";

  write_message(std::move(relu_result_msg));
  return;
}

void ngraph::he::HESealClient::decode_to_real_vec(const seal::Plaintext& plain,
                                                  std::vector<double>& output,
                                                  bool complex) {
  assert(output.size() == 0);
  if (complex) {
    std::vector<std::complex<double>> complex_outputs;
    m_ckks_encoder->decode(plain, complex_outputs);
    assert(complex_outputs.size() >= m_batch_size);
    complex_outputs.resize(m_batch_size);
    complex_vec_to_real_vec(output, complex_outputs);
  } else {
    m_ckks_encoder->decode(plain, output);
    assert(m_batch_size <= output.size());
    output.resize(m_batch_size);
  }
}


#define BOOST_TEST_MODULE pcraster com single_valued_raster
#include <boost/test/unit_test.hpp>
#include "csftypes.h"
#include "com_singlevaluedraster.h"

BOOST_AUTO_TEST_CASE(this_)
{
  using namespace com;

  SingleValuedRaster<UINT1> v(4,5,8);
  BOOST_CHECK(v.nrRows()==4);
  BOOST_CHECK(v.nrCols()==5);
  BOOST_CHECK(v.cell(0,0)==8);
  BOOST_CHECK(v.cell(3,4)==8);

  v.setMV(0,2);

  BOOST_CHECK(v.cell(0,0)==MV_UINT1);
  BOOST_CHECK(v.isMV(0,0));
  BOOST_CHECK(v.isMV(2,0));
}

BOOST_AUTO_TEST_CASE(i_raster)
{
  using namespace com;
}


#ifndef GUARD_CIRCUIT_SUPER_HPP
#define GUARD_CIRCUIT_SUPER_HPP

//REVIEW need to see how order of include is affecting the ability to compile
//program
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "circuit_structure.hpp"
#include "circuit_source.hpp"
#include "circuit_linear.hpp"
#include "circuit_diode.hpp"
#include "circuit_transistor.hpp"
#include "circuit_math.hpp"
#include "circuit_simulator.hpp"
#include "circuit_parser.hpp"
#endif


#include <iostream>

#include <boost/date_time.hpp>
#include <boost/uuid/uuid.hpp>            // uuid class
#include <boost/uuid/uuid_generators.hpp> // generators
#include <boost/uuid/uuid_io.hpp>         // streaming operators etc.

void dateExample() {
    using namespace boost::gregorian;
	date firstEarthDay( 1970, Apr, 22);
	date sputnikFirstOrbit( 1957, Oct, 4);
	date berlinWallFell( 1989, Nov, 10);

    date today = day_clock::local_day();
    std::cout << today << std::endl;
} // end dateEx()

int main()
{
    // output dates
	dateExample();

    //output uuids
    boost::uuids::random_generator generator;
    boost::uuids::uuid uuid1 = generator();
    std::cout << uuid1 << std::endl;
    boost::uuids::uuid uuid2 = generator();
    std::cout << uuid2 << std::endl;

    return 0;
}


// Copyright (C) 2015, Pawel Tomulik <ptomulik@meil.pw.edu.pl>
//
// 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)
#define BOOST_TEST_MODULE test_txpl_vm_eval_unary_op

#include <txpl/test_config.hpp>
#include <boost/test/unit_test.hpp>

#ifndef TXPL_TEST_SKIP_VM_EVAL_UNARY_OP

#include <txpl/vm/eval_unary_op.hpp>
#include <txpl/vm/basic_types.hpp>
#include <txpl/vm/value.hpp>
#include <txpl/vm/array.hpp>
#include <txpl/vm/object.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/get.hpp>

BOOST_AUTO_TEST_CASE(plus_)
{
  using namespace txpl::vm;
  typedef basic_types<>::char_type   char_type;
  typedef basic_types<>::int_type    int_type;
  typedef basic_types<>::bool_type   bool_type;
  typedef basic_types<>::real_type   real_type;
  typedef basic_types<>::string_type string_type;
  typedef basic_types<>::regex_type  regex_type;
  typedef basic_types<>::blank_type  blank_type;
  typedef array<> array_type;
  typedef object<> object_type;
  value<> r;
  auto op = eval_unary_op<op_t::plus_>(r);
  {
    r = blank_type();
    const value<> v = char_type{'a'};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == +char_type{'a'});
  }
  {
    r = blank_type();
    const value<> v = int_type{100};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == +int_type{100});
  }
  {
    r = blank_type();
    const value<> v = bool_type{true};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == +bool_type{true});
  }
  {
    r = blank_type();
    const value<> v = real_type{.123};
    BOOST_CHECK(boost::apply_visitor(op, v));
    real_type x = real_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == +real_type{.123});
  }
  {
    r = blank_type();
    const value<> v = string_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = regex_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = array_type();
    BOOST_CHECK(boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<array_type>(r));
  }
  {
    r = blank_type();
    const value<> v = object_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(minus_)
{
  using namespace txpl::vm;
  typedef basic_types<>::char_type   char_type;
  typedef basic_types<>::int_type    int_type;
  typedef basic_types<>::bool_type   bool_type;
  typedef basic_types<>::real_type   real_type;
  typedef basic_types<>::string_type string_type;
  typedef basic_types<>::regex_type  regex_type;
  typedef basic_types<>::blank_type  blank_type;
  typedef array<> array_type;
  typedef object<> object_type;

  value<> r;
  auto op = eval_unary_op<op_t::minus_>(r);
  {
    r = blank_type();
    const value<> v = char_type{'a'};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == -char_type{'a'});
  }
  {
    r = blank_type();
    const value<> v = int_type{100};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == -int_type{100});
  }
  {
    r = blank_type();
    const value<> v = bool_type{true};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == -bool_type{true});
  }
  {
    r = blank_type();
    const value<> v = real_type{.123};
    BOOST_CHECK(boost::apply_visitor(op, v));
    real_type x = real_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == -real_type{.123});
  }
  {
    r = blank_type();
    const value<> v = string_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = regex_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = array_type();
    BOOST_CHECK(boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<array_type>(r));
  }
  {
    r = blank_type();
    const value<> v = object_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(not_)
{
  using namespace txpl::vm;
  typedef basic_types<>::char_type   char_type;
  typedef basic_types<>::int_type    int_type;
  typedef basic_types<>::bool_type   bool_type;
  typedef basic_types<>::real_type   real_type;
  typedef basic_types<>::string_type string_type;
  typedef basic_types<>::regex_type  regex_type;
  typedef basic_types<>::blank_type  blank_type;
  typedef array<> array_type;
  typedef object<> object_type;
  value<> r;
  auto op = eval_unary_op<op_t::not_>(r);
  {
    r = blank_type();
    const value<> v = char_type{'a'};
    BOOST_CHECK(boost::apply_visitor(op, v));
    bool_type x = bool_type{true};
    BOOST_CHECK_NO_THROW(x = boost::get<bool_type>(r));
    BOOST_CHECK(x == !char_type{'a'});
  }
  {
    r = blank_type();
    const value<> v = int_type{100};
    BOOST_CHECK(boost::apply_visitor(op, v));
    bool_type x = bool_type{true};
    BOOST_CHECK_NO_THROW(x = boost::get<bool_type>(r));
    BOOST_CHECK(x == !int_type{100});
  }
  {
    r = blank_type();
    const value<> v = bool_type{true};
    BOOST_CHECK(boost::apply_visitor(op, v));
    bool_type x = bool_type{true};
    BOOST_CHECK_NO_THROW(x = boost::get<bool_type>(r));
    BOOST_CHECK(x == !bool_type{true});
  }
  {
    r = blank_type();
    const value<> v = real_type{.123};
    BOOST_CHECK(boost::apply_visitor(op, v));
    bool_type x = bool_type{true};
    BOOST_CHECK_NO_THROW(x = boost::get<bool_type>(r));
    BOOST_CHECK(x == !real_type{.123});
  }
  {
    r = blank_type();
    const value<> v = string_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = regex_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = array_type();
    BOOST_CHECK(boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<array_type>(r));
  }
  {
    r = blank_type();
    const value<> v = object_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(neg_)
{
  using namespace txpl::vm;
  typedef basic_types<>::char_type   char_type;
  typedef basic_types<>::int_type    int_type;
  typedef basic_types<>::bool_type   bool_type;
  typedef basic_types<>::real_type   real_type;
  typedef basic_types<>::string_type string_type;
  typedef basic_types<>::regex_type  regex_type;
  typedef basic_types<>::blank_type  blank_type;
  typedef array<> array_type;
  typedef object<> object_type;
  value<> r;
  auto op = eval_unary_op<op_t::neg_>(r);
  {
    r = blank_type();
    const value<> v = char_type{'a'};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{'x'};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == ~char_type{'a'});
  }
  {
    r = blank_type();
    const value<> v = int_type{100};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == ~int_type{100});
  }
  {
    r = blank_type();
    const value<> v = bool_type{true};
    BOOST_CHECK(boost::apply_visitor(op, v));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == ~bool_type{true});
  }
  {
    r = blank_type();
    const value<> v = real_type{.123};
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = string_type();
    BOOST_CHECK(~boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = regex_type();
    BOOST_CHECK(~boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
  {
    r = blank_type();
    const value<> v = array_type();
    BOOST_CHECK(boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<array_type>(r));
  }
  {
    r = blank_type();
    const value<> v = object_type();
    BOOST_CHECK(!boost::apply_visitor(op, v));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
#else
BOOST_AUTO_TEST_CASE(dummy)
{
  BOOST_CHECK(true);
}
#endif


#include "sparse_grid_dynamic.hpp"
#include <boost/python.hpp>
#include "datatypes.hpp"

namespace gd {
using namespace boost::python;

template<class T>
	void wrap_eval(T& t, object callback) {
		auto f = [&](int dim, double* point) -> double {
			object result;
			if(dim == 1)
				result = callback(point[0]);
			if(dim == 2)
				result = callback(point[0], point[1]);
			if(dim == 3)
				result = callback(point[0], point[1], point[2]);
			if(dim == 4)
				result = callback(point[0], point[1], point[2], point[3]);
			if(dim == 5)
				result = callback(point[0], point[1], point[2], point[3], point[4]);
			if(dim == 6)
				result = callback(point[0], point[1], point[2], point[3], point[4], point[5]);
			extract<double> value(result);
			return value();
		};
		t.eval(f);
	}

template<class T, typename... X>
	double wrap_call(T& t, X... x) {
		/*auto f = [&](X... x) -> double {
			object result = callback(x...);
			extract<double> value(result);
			return value();
		};
		t.eval(f);*/
		double xarray[sizeof...(X)] = {x...};
		return t(xarray);
		//for(int i = 0; i <<
	}

template<class T, typename X0, typename... X>
	void wrap_call_vec(T& t, X0 x0, X... x) {
		/*auto f = [&](X... x) -> double {
			object result = callback(x...);
			extract<double> value(result);
			return value();
		};
		t.eval(f);*/
		const int dim = 1+sizeof...(X);
		X0* xarray[dim] = {&x0, &x...};
		int indices[dim] = {0};
		int count[dim];
		double point[dim];
		for(int d = 0; d < dim; d++) {
			count[d] = xarray[d]->size();
			printf("count[%d] = %d\n", d, count[d]);
			point[d] = xarray[d]->operator[](0);
		}
//for(int i = 0; i <<
		bool done = false;
		while(!done) { // loop over indices
			t(point);
			for(int d = dim-1; d >= 0; d--) {
				indices[d]++;

				if(indices[d]>=count[d]) {
					indices[d] = 0;
					if(d == 0) done = true;
					else
					point[d] = xarray[d]->operator[](indices[d]);
				} else {
					point[d] = xarray[d]->operator[](indices[d]);
					break;
				}
			}
		}
	}

void py_export_sparse_grid_dynamic() {
{
	/*class_<BasisSet, boost::noncopyable>("BasisSet", no_init);
	class_<BasisSetHierTriangular, bases<BasisSet>, boost::noncopyable >("BasisSetHierTriangular", init<>())
		.def("count", &BasisSetHierTriangular::count)
		.def("__call__", &BasisSetHierTriangular::operator())
		;
	*/
	class_<BasisSetHierTriangular, boost::noncopyable >("BasisSetHierTriangulara", init<>())
		.def("count", &BasisSetHierTriangular::count)
		.def("__call__", &BasisSetHierTriangular::operator())
		;
	class_<BasisSetHierTriangularM, boost::noncopyable >("BasisSetHierTriangular", init<>())
		.def("count", &BasisSetHierTriangularM::count)
		.def("center", &BasisSetHierTriangularM::center)
		.def("__call__", &BasisSetHierTriangularM::operator())
		;

	{
		typedef SparseGrid T;
		class_<T>("SparseGridD", init<int,int>())
			//.def("test", &T::test);
			.def("marginalize1d", &T::marginalize1d)
			.def("marginalize2d", &T::marginalize2d)
			.def("integrate", &T::integrate)
			.def("eval", &wrap_eval<T>)
			.def("__call__", &wrap_call<T, double>)
			.def("__call__", &wrap_call<T, double, double>)
			.def("__call__", &wrap_call<T, double, double, double>)
			.def("__call__", &wrap_call<T, double, double, double, double>)
			.def("__call__", &wrap_call<T, double, double, double, double, double>)
			.def("__call__", &wrap_call<T, double, double, double, double, double, double>)
			.def("__call__", &wrap_call_vec<T, double_vector>)
			.def("__call__", &wrap_call_vec<T, double_vector, double_vector>)
			.def("__call__", &wrap_call_vec<T, double_vector, double_vector, double_vector>)
			.def("__call__", &wrap_call_vec<T, double_vector, double_vector, double_vector, double_vector>)
			.def("__call__", &wrap_call_vec<T, double_vector, double_vector, double_vector, double_vector, double_vector>)
			.def("__call__", &wrap_call_vec<T, double_vector, double_vector, double_vector, double_vector, double_vector, double_vector>)
//.def("print_", &T::print)
			//.def("eval", &wrap_eval<T, double, double>)
	//.def("test", &T::test)
			;
	}
	{
		typedef RegularGrid T;
		class_<T>("RegularGridD", init<int,int>())
			//.def("test", &T::test);
			.def("eval", &wrap_eval<T>)
			.def("__call__", &wrap_call<T, double>)
			.def("__call__", &wrap_call<T, double, double>)
			.def("__call__", &wrap_call<T, double, double, double>)
			.def("__call__", &wrap_call<T, double, double, double, double>)
			.def("__call__", &wrap_call<T, double, double, double, double, double>)
			.def("__call__", &wrap_call<T, double, double, double, double, double, double>)
//.def("print_", &T::print)
			//.def("eval", &wrap_eval<T, double, double>)
	//.def("test", &T::test)
			;
	}
}
}
}


////////////////////////////////////////////////////////////////////////////////
// ArrayPadder.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Add padding to an Eigen matrix.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  09/10/2019 14:08:08
////////////////////////////////////////////////////////////////////////////////
#ifndef ARRAYPADDER_HH
#define ARRAYPADDER_HH
#include <type_traits>
#include <Eigen/Dense>

template<int paddingCols, typename Derived, typename T = typename Derived::Scalar, typename = typename std::enable_if<(Derived::ColsAtCompileTime > 0), void>::type>
auto pad_columns(const Eigen::DenseBase<Derived> &mat, T value = 0.0) {
    using MType = Eigen::Matrix<typename Derived::Scalar,
                                Derived::RowsAtCompileTime,
                                Derived::ColsAtCompileTime + paddingCols>;
    return MType::NullaryExpr(mat.rows(), mat.cols() + paddingCols, [&mat, value](Eigen::Index i, Eigen::Index j) {
                if (j < Derived::ColsAtCompileTime) return mat(i, j);
                return value;
            });
}

#endif /* end of include guard: ARRAYPADDER_HH */