#define BOOST_TEST_MODULE "test_read_dihedral_angle_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/input/read_local_interaction.hpp>

BOOST_AUTO_TEST_CASE(read_dihedral_angle_go_contact)
{
    mjolnir::LoggerManager::set_default_logger("test_read_dihedral_angle_interaction.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = traits_type::real_type;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction = "DihedralAngle"
            potential   = "ClementiDihedral"
            topology    = "none"
            parameters  = []
        )"_toml;

        const auto base = mjolnir::read_local_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv = dynamic_cast<mjolnir::DihedralAngleInteraction<
            traits_type, mjolnir::ClementiDihedralPotential<real_type>>*
            >(base.get()); // check the expected type is contained
        BOOST_TEST(static_cast<bool>(derv));
    }
}

BOOST_AUTO_TEST_CASE(read_dihedral_angle_gaussian)
{
    mjolnir::LoggerManager::set_default_logger("test_read_dihedral_angle_interaction.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = traits_type::real_type;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction = "DihedralAngle"
            potential   = "Gaussian"
            topology    = "none"
            parameters  = []
        )"_toml;

        const auto base = mjolnir::read_local_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv = dynamic_cast<mjolnir::DihedralAngleInteraction<
            traits_type, mjolnir::PeriodicGaussianPotential<real_type>>*
            >(base.get()); // check the expected type is contained
        BOOST_TEST(static_cast<bool>(derv));
    }
}

BOOST_AUTO_TEST_CASE(read_dihedral_angle_flexible_local)
{
    mjolnir::LoggerManager::set_default_logger("test_read_dihedral_angle_interaction.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = traits_type::real_type;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction = "DihedralAngle"
            potential   = "FlexibleLocalDihedral"
            topology    = "none"
            parameters  = []
        )"_toml;

        const auto base = mjolnir::read_local_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv = dynamic_cast<mjolnir::DihedralAngleInteraction<
            traits_type, mjolnir::FlexibleLocalDihedralPotential<real_type>>*
            >(base.get()); // check the expected type is contained
        BOOST_TEST(static_cast<bool>(derv));
    }
}

BOOST_AUTO_TEST_CASE(read_dihedral_angle_cosine)
{
    mjolnir::LoggerManager::set_default_logger("test_read_dihedral_angle_interaction.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = traits_type::real_type;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction = "DihedralAngle"
            potential   = "Cosine"
            topology    = "none"
            parameters  = []
        )"_toml;

        const auto base = mjolnir::read_local_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv = dynamic_cast<mjolnir::DihedralAngleInteraction<
            traits_type, mjolnir::CosinePotential<real_type>>*
            >(base.get()); // check the expected type is contained
        BOOST_TEST(static_cast<bool>(derv));
    }
}


// This file is part of LatticeTester.
//
// LatticeTester
// Copyright (C) 2012-2018  Pierre L'Ecuyer and Universite de Montreal
//
// 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.

/*
 * The purpose of this example is to compare the time
 *  of each reduction method. We compute several
 * random matrix and apply algorithms. The parameters of this
 * analysis are defined at the beginning of this file,
 * after include part. The output is a graphic generated by R.
 * The User need R distribution and the header RInside.h
 */

// Include Header
#include <iostream>
#include <map>
#include <fstream>
#include <iterator>
#include <string>
#include <sstream>
#include <iomanip>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>

// Include LatticeTester Header
#include "latticetester/Util.h"
#include "latticetester/Const.h"
#include "latticetester/IntFactor.h"
#include "latticetester/IntLatticeBasis.h"
#include "latticetester/Reducer.h"
#include "latticetester/ParamReader.h"
#include "latticetester/Config.h"
#include "latticetester/LatticeAnalysis.h"

// Include NTL Header
#include <NTL/tools.h>
#include <NTL/ctools.h>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include "NTL/vec_ZZ.h"
#include "NTL/vec_ZZ_p.h"
#include <NTL/vec_vec_ZZ.h>
#include <NTL/vec_vec_ZZ_p.h>
#include <NTL/mat_ZZ.h>
#include <NTL/matrix.h>
#include <NTL/LLL.h>

using namespace std;
using namespace LatticeTester;

int ZZRR(int argc, char* argv[]) {
  LatticeAnalysis<NTL::ZZ, NTL::ZZ, NTL::RR, NTL::RR> latAnalysis;
  struct stat buf; // properties of a file or directory
  int status = 0;

  for (int j = 2; j < argc; j++) {
    // Do the test for each data file or directory on the command line

    stat(argv[j], &buf);
    if (0 != S_ISDIR(buf.st_mode)) // directory
      status |= latAnalysis.doTestFromDirectory (argv[j]);
    else {
      string dataname(argv[j]);
      dataname.append(".dat");
      stat(dataname.c_str(), &buf);
      status |= latAnalysis.doTestFromInputFile (argv[j]);
    }
  }
  return 1;
}
int ZZDD(int argc, char* argv[]) {
  LatticeAnalysis<NTL::ZZ, NTL::ZZ, double, double> latAnalysis;
  struct stat buf; // properties of a file or directory
  int status = 0;

  for (int j = 2; j < argc; j++) {
    // Do the test for each data file or directory on the command line

    stat(argv[j], &buf);
    if (0 != S_ISDIR(buf.st_mode)) // directory
      status |= latAnalysis.doTestFromDirectory (argv[j]);
    else {
      string dataname(argv[j]);
      dataname.append(".dat");
      stat(dataname.c_str(), &buf);
      status |= latAnalysis.doTestFromInputFile (argv[j]);
    }
  }
  return 1;
}
int LLDD(int argc, char* argv[]) {
  LatticeAnalysis<std::int64_t, std::int64_t, double, double> latAnalysis;
  struct stat buf; // properties of a file or directory
  int status = 0;

  for (int j = 2; j < argc; j++) {
    // Do the test for each data file or directory on the command line

    stat(argv[j], &buf);
    if (0 != S_ISDIR(buf.st_mode)) // directory
      status |= latAnalysis.doTestFromDirectory (argv[j]);
    else {
      string dataname(argv[j]);
      dataname.append(".dat");
      stat(dataname.c_str(), &buf);
      status |= latAnalysis.doTestFromInputFile (argv[j]);
    }
  }
  return 1;
}
//==============================================================================

int main (int argc, char *argv[])
{
  std::clock_t start_t = std::clock();
  if (argc < 2) {
    cerr << "\n*** Usage:\n   "
      << argv[0] << " data_file1 data_file2 ...." << endl
      << "or\n   "
      << argv[0] << " dir1 dir2 ...." << endl
      << endl;
    return -1;
  } else if (argc < 3) {
    cerr << "Must specify types\n";
    return -1;
  }
  //cout << "ARGC " << argc << endl;
  if (!strcmp(argv[1], "ZZRR")) {
    ZZRR(argc, argv);
    std::clock_t end_t = std::clock();
    cout << "Time: " << (double)(end_t-start_t)/CLOCKS_PER_SEC << " sec\n";
  } else if (!strcmp(argv[1], "ZZDD")) {
    ZZDD(argc, argv);
    std::clock_t end_t = std::clock();
    cout << "Time: " << (double)(end_t-start_t)/CLOCKS_PER_SEC << " sec\n";
  } else if (!strcmp(argv[1], "LLDD")) {
    LLDD(argc, argv);
    std::clock_t end_t = std::clock();
    cout << "Time: " << (double)(end_t-start_t)/CLOCKS_PER_SEC << " sec\n";
  } else {
    cerr << "No valid type name has been specified\n";
  }

  return 0;
}


#include <boost/geometry/formulas/differential_quantities.hpp>


/*
 * scan_varying_ray_predictor.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/spot_prediction/scan_varying_ray_predictor.h>
#include <dxtbx/model/sequence.h>
#include <dxtbx/model/beam.h>
#include <dxtbx/model/goniometer.h>
#include <dxtbx/model/detector.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;

  static object predict1(const ScanVaryingRayPredictor &predictor,
                         const cctbx::miller::index<> &h,
                         const mat3<double> &A1,
                         const mat3<double> &A2,
                         int image,
                         std::size_t step) {
    boost::optional<Ray> result = predictor(h, A1, A2, image, step);
    return !result ? object() : object(*result);
  }

  static object predict2(const ScanVaryingRayPredictor &predictor,
                         const cctbx::miller::index<> &h,
                         const mat3<double> &A1,
                         const mat3<double> &A2,
                         const vec3<double> &s0a,
                         const vec3<double> &s0b,
                         int image,
                         std::size_t step) {
    boost::optional<Ray> result = predictor(h, A1, A2, s0a, s0b, image, step);
    return !result ? object() : object(*result);
  }

  void export_scan_varying_ray_predictor() {
    // Create and return the wrapper for the spot predictor object
    class_<ScanVaryingRayPredictor>("ScanVaryingRayPredictor", no_init)
      .def(init<vec3<double>, vec3<double>, int, vec2<double>, double>(
        (arg("s0"), arg("m2"), arg("frame0"), arg("dphi"), arg("dmin"))))
      .def("__call__",
           &predict1,
           (arg("hkl"), arg("A1"), arg("A2"), arg("image"), arg("step") = 1))
      .def("__call__",
           &predict2,
           (arg("hkl"),
            arg("A1"),
            arg("A2"),
            arg("s0a"),
            arg("s0b"),
            arg("image"),
            arg("step") = 1));
  }

}}}  // namespace dials::algorithms::boost_python


#include <cassert>

#include <iostream>
#include <boost/filesystem.hpp>

#include <photon_propagator/device.hpp>
#include <photon_propagator/geometry.hpp>
#include <photon_propagator/configuration.hpp>
#include <photon_propagator/optical_module_lines.hpp>
#include <photon_propagator/tracks.hpp>
#include <photon_propagator/particle.hpp>
#include <photon_propagator/propagator.hpp>

using std::cerr;
using std::endl;

using boost::filesystem::path;
using boost::filesystem::exists;

int main(int argc, char* argv[]){

  if(argc != 3){
    std::cerr<<"Usage: test_geometry <GEOMETRY_FILE> <ICE_MODEL_PATH>"<<std::endl;
    std::exit(-1);
  }

  path geometry_filename(argv[1]);
  assert(exists(path(geometry_filename)));

  path ice_model_path(argv[2]);
  assert(exists(path(ice_model_path)));

  std::shared_ptr<Device> device(new Device(0));

  Geometry geometry(geometry_filename);
  Configuration config(ice_model_path);
  OpticalModuleLines om_lines(geometry, config);

  float scattering_angle{config.get_configuration().g};
  IceModel ice_model(ice_model_path, scattering_angle);

  size_t photon_buffer_size{1000};
  size_t hit_buffer_size{100};

  particle electron;
  electron.major_id=0;
  electron.minor_id=0;
  electron.ptype=particle_type::e_minus;
  electron.time=0.;
  electron.energy=100.; // in god-given units
  electron.length=0.;
  electron.direction={0,0,0};
  electron.position={0,0,0};

  // These should know how to generate their own photons.
  Tracks tracks(n_tracks, device);
  Cascades cascades(n_cascades, device);

  Propagator propagator(device,
			config,
			ice_model,
			geometry,
			om_lines,
			hit_buffer_size,
			photon_buffer_size);

  propagator.propagate(photons);

}


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

    @Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/detail/make_dependent.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/bitwise_ornot.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/if_else_allbits.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/constant/ratio.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2: a shift is out of range");
      return _mm_srai_epi16(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2: a shift is out of range");
      return _mm_srli_epi16(a0, a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2: a shift is out of range");
      return _mm_srli_epi32(a0, int(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2: a shift is out of range");
      return _mm_srai_epi32(a0, int(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2 int64: a shift is out of range");
      A0 that = _mm_srli_epi64(a0, int(a1));
      A0 mask = _mm_srli_epi64(Allbits<A0>(), int(a1));
      return bitwise_ornot(that, if_else_allbits(is_ltz(a0), mask));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2 uint64: a shift is out of range");
      return _mm_srli_epi64(a0, int(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2 int8: a shift is out of range");
      auto s = split(a0);
      return bitwise_cast<A0>(group(shift_right(s[0], a1), shift_right(s[1], a1)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_right_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right sse2 uint8: a shift is out of range");
      using gen_t = typename A0::template retype<std::int16_t,A0::static_size/2>;
      A0 const Mask1 = bitwise_cast<A0>(gen_t(0x00ff));
      A0 const Mask2 = bitwise_cast<A0>(gen_t(0xff00));

      A0 tmp  = bitwise_and(a0, Mask1);
      A0 tmp1 = _mm_srli_epi16(tmp, int(a1));
      tmp1 = bitwise_and(tmp1, Mask1);
      tmp = bitwise_and(a0, Mask2);
      A0 tmp3 = _mm_srli_epi16(tmp, int(a1));
      return bitwise_or(tmp1, bitwise_and(tmp3, Mask2));
    }
  };
} } }

#endif


/* git    Copyright (c) 2010-2018, 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 <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerEphemeris.h"

#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"
#include "Tudat/SimulationSetup/EstimationSetup/variationalEquationsSolver.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EstimationSetup/createNumericalSimulator.h"
#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h"

namespace tudat
{

namespace unit_tests
{

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

BOOST_AUTO_TEST_SUITE( test_variational_equation_calculation )

template< typename TimeType = double , typename StateScalarType  = double >
        std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > >
executeEarthMoonSimulation(
        const std::vector< std::string > centralBodies,
        const Eigen::Matrix< StateScalarType, 12, 1 > initialStateDifference =
        Eigen::Matrix< StateScalarType, 12, 1 >::Zero( ),
        const int propagationType = 0,
        const Eigen::Vector3d parameterPerturbation = Eigen::Vector3d::Zero( ),
        const bool propagateVariationalEquations = 1 )
{

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

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

    // Specify initial time
    TimeType initialEphemerisTime = TimeType( 1.0E7 );
    TimeType finalEphemerisTime = initialEphemerisTime + 0.5E7;
    double maximumTimeStep = 3600.0;

    double buffer = 10.0 * maximumTimeStep;

    // Create bodies needed in simulation
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );

    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Set accelerations between bodies that are to be taken into account.
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfEarth;
    accelerationsOfEarth[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfEarth[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Earth" ] = accelerationsOfEarth;

    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
    accelerationsOfMoon[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfMoon[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Moon" ] = accelerationsOfMoon;

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

    unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( );

    // Define propagator settings.
    std::map< std::string, std::string > centralBodyMap;

    for( unsigned int i = 0; i < numberOfNumericalBodies; i++ )
    {
        centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ];
    }

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

    // Create integrator settings
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings =
            std::make_shared< IntegratorSettings< TimeType > >
            ( rungeKutta4, TimeType( initialEphemerisTime ), 1800.0 );

    // Set initial states of bodies to integrate.
    TimeType initialIntegrationTime = initialEphemerisTime;

    // Set (perturbed) initial state.
    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialTranslationalState;
    initialTranslationalState = getInitialStatesOfBodies< TimeType, StateScalarType >(
                bodiesToIntegrate, centralBodies, bodyMap, initialIntegrationTime );
    initialTranslationalState += initialStateDifference;

    // Create propagator settings
    std::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings;
    TranslationalPropagatorType propagatorType;
    if( propagationType == 0 )
    {
        propagatorType = cowell;
    }
    else if( propagationType == 1 )
    {
        propagatorType = encke;
    }
    propagatorSettings =  std::make_shared< TranslationalStatePropagatorSettings< StateScalarType > >
            ( centralBodies, accelerationModelMap, bodiesToIntegrate, initialTranslationalState,
              TimeType( finalEphemerisTime ), propagatorType );

    // Define parameters.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    {
        parameterNames.push_back(
                    std::make_shared< InitialTranslationalStateEstimatableParameterSettings< StateScalarType > >(
                        "Moon", propagators::getInitialStateOfBody< TimeType, StateScalarType >(
                            "Moon", centralBodies[ 0 ], bodyMap, TimeType( initialEphemerisTime ) ) +
                    initialStateDifference.segment( 0, 6 ),
                    centralBodies[ 0 ] ) );
        parameterNames.push_back(
                    std::make_shared< InitialTranslationalStateEstimatableParameterSettings< StateScalarType > >(
                        "Earth", propagators::getInitialStateOfBody< TimeType, StateScalarType >(
                            "Earth", centralBodies[ 1 ], bodyMap, TimeType( initialEphemerisTime ) ) +
                    initialStateDifference.segment( 6, 6 ),
                    centralBodies[ 1 ] ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Moon", gravitational_parameter ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Earth", gravitational_parameter ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Sun", gravitational_parameter ) );

    }

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

    // Perturb parameters.
    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > parameterVector =
            parametersToEstimate->template getFullParameterValues< StateScalarType >( );
    parameterVector.block( 12, 0, 3, 1 ) += parameterPerturbation;
    parametersToEstimate->resetParameterValues( parameterVector );

    std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
            std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > results;

    {
        // Create dynamics simulator
        SingleArcVariationalEquationsSolver< StateScalarType, TimeType > dynamicsSimulator =
                SingleArcVariationalEquationsSolver< StateScalarType, TimeType >(
                    bodyMap, integratorSettings, propagatorSettings, parametersToEstimate,
                    1, std::shared_ptr< numerical_integrators::IntegratorSettings< double > >( ), 1, 0 );

        // Propagate requested equations.
        if( propagateVariationalEquations )
        {
            dynamicsSimulator.integrateVariationalAndDynamicalEquations( propagatorSettings->getInitialStates( ), 1 );
        }
        else
        {
            dynamicsSimulator.integrateDynamicalEquationsOfMotionOnly( propagatorSettings->getInitialStates( ) );
        }

        // Retrieve test data
        double testEpoch = 1.4E7;
        Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > testStates =
                Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( 12 );
        testStates.block( 0, 0, 6, 1 ) = bodyMap[ "Moon" ]->getStateInBaseFrameFromEphemeris( testEpoch );
        if( centralBodyMap[ "Moon" ] == "Earth" )
        {
            testStates.block( 0, 0, 6, 1 ) -= bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( testEpoch );
        }

        testStates.block( 6, 0, 6, 1 ) = bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( testEpoch );

        if( propagateVariationalEquations )
        {
            results.first.push_back( dynamicsSimulator.getStateTransitionMatrixInterface( )->
                                     getCombinedStateTransitionAndSensitivityMatrix( testEpoch ) );
        }
        results.second.push_back( testStates );
    }
    return results;
}

//! Test the state transition and sensitivity matrix computation against their numerical propagation.
/*!
 *  Test the state transition and sensitivity matrix computation against their numerical propagation. This unit test
 *  propagates the variational equations for the Earth and Moon, using both a barycentric origin and hierarchical origin.
 *  For the hierarchical origin, both an Encke and Cowell propagator are used. The results are compared against
 *  results obtained from numerical differentiation (first-order central difference). *
 */
BOOST_AUTO_TEST_CASE( testEarthMoonVariationalEquationCalculation )
{
    std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput;

    std::vector< std::vector< std::string > > centralBodiesSet;
    std::vector< std::string > centralBodies;

    // Define central bodt settings
    centralBodies.resize( 2 );

    centralBodies[ 0 ] = "SSB";
    centralBodies[ 1 ] = "SSB";
    centralBodiesSet.push_back( centralBodies );

    centralBodies[ 0 ] = "Earth";
    centralBodies[ 1 ] = "Sun";
    centralBodiesSet.push_back( centralBodies );

    // Define variables for numerical differentiation
    Eigen::Matrix< double, 12, 1>  perturbedState;
    Eigen::Vector3d perturbedParameter;

    Eigen::Matrix< double, 12, 1> statePerturbation;
    Eigen::Vector3d parameterPerturbation;

    for( unsigned int i = 0; i < centralBodiesSet.size( ); i++ )
    {
        // Define parameter perturbation
        parameterPerturbation  = ( Eigen::Vector3d( ) << 1.0E10, 1.0E10, 1.0E14 ).finished( );

        // Define state perturbation
        if( i == 0 )
        {
            statePerturbation = ( Eigen::Matrix< double, 12, 1>( ) <<
                                  100000.0, 100000.0, 100000.0, 0.1, 0.1, 0.1,
                                  100000.0, 100000.0, 100000.0, 0.1, 0.1, 0.1 ).finished( );
        }
        else if( i == 1 )
        {
            statePerturbation = ( Eigen::Matrix< double, 12, 1>( ) <<
                                  100000.0, 100000.0, 100000.0, 0.1, 0.1, 0.1,
                                  100000.0, 100000.0, 10000000.0, 0.1, 0.1, 10.0 ).finished( );
        }

        // Only perform Encke propagation is origin is not SSB
        unsigned int maximumPropagatorType = 1;
        if( i == 1 )
        {
            maximumPropagatorType = 2;
        }

        // Test for all requested propagator types.
        for( unsigned int k = 0; k < maximumPropagatorType; k++ )
        {
            // Compute state transition and sensitivity matrices
            currentOutput = executeEarthMoonSimulation< double, double >(
                        centralBodiesSet[ i ], Eigen::Matrix< double, 12, 1 >::Zero( ), k );
            Eigen::MatrixXd stateTransitionAndSensitivityMatrixAtEpoch = currentOutput.first.at( 0 );
            Eigen::MatrixXd manualPartial = Eigen::MatrixXd::Zero( 12, 15 );

            // Numerically compute state transition matrix
            for( unsigned int j = 0; j < 12; j++ )
            {
                Eigen::VectorXd upPerturbedState, downPerturbedState;
                perturbedState.setZero( );
                perturbedState( j ) += statePerturbation( j );
                upPerturbedState = executeEarthMoonSimulation< double, double >(
                            centralBodiesSet[ i ], perturbedState, k, Eigen::Vector3d::Zero( ), 0 ).second.at( 0 );

                perturbedState.setZero( );
                perturbedState( j ) -= statePerturbation( j );
                downPerturbedState = executeEarthMoonSimulation< double, double >(
                            centralBodiesSet[ i ], perturbedState, k, Eigen::Vector3d::Zero( ), 0 ).second.at( 0 );
                manualPartial.block( 0, j, 12, 1 ) =
                        ( upPerturbedState - downPerturbedState ) / ( 2.0 * statePerturbation( j ) );
            }

            // Numerically compute sensitivity matrix
            for( unsigned int j = 0; j < 3; j ++ )
            {
                Eigen::VectorXd upPerturbedState, downPerturbedState;
                perturbedState.setZero( );
                Eigen::Vector3d upPerturbedParameter, downPerturbedParameter;
                perturbedParameter.setZero( );
                perturbedParameter( j ) += parameterPerturbation( j );
                upPerturbedState = executeEarthMoonSimulation< double, double >(
                            centralBodiesSet[ i ], perturbedState, k, perturbedParameter, 0 ).second.at( 0 );

                perturbedParameter.setZero( );
                perturbedParameter( j ) -= parameterPerturbation( j );
                downPerturbedState = executeEarthMoonSimulation< double, double >(
                            centralBodiesSet[ i ], perturbedState, k, perturbedParameter, 0 ).second.at( 0 );
                manualPartial.block( 0, j + 12, 12, 1 ) =
                        ( upPerturbedState - downPerturbedState ) / ( 2.0 * parameterPerturbation( j ) );
            }

            // Check results
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        stateTransitionAndSensitivityMatrixAtEpoch.block( 0, 0, 12, 15 ), manualPartial, 2.0E-4 );

        }

    }
}

template< typename TimeType = double , typename StateScalarType  = double >
std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > >
executeOrbiterSimulation(
        const Eigen::Matrix< StateScalarType, 6, 1 > initialStateDifference =
        Eigen::Matrix< StateScalarType, 6, 1 >::Zero( ),
        const Eigen::VectorXd parameterPerturbation = Eigen::VectorXd::Zero( 10 ),
        const bool propagateVariationalEquations = 1 )
{
    int numberOfParametersToEstimate = 10;

    //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 initial time
    TimeType initialEphemerisTime = TimeType( 1.0E7 );
    TimeType finalEphemerisTime = initialEphemerisTime + 4.0 * 3600.0;

    // Create bodies needed in simulation
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames );
    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, "SSB", "ECLIPJ2000" );

    // 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 ) );
    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 );

    // Create integrator settings
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings =
            std::make_shared< IntegratorSettings< TimeType > >
            ( rungeKutta4, TimeType( initialEphemerisTime ), 5.0 );

    // Set Keplerian elements for Asterix.
    Eigen::Vector6d asterixInitialStateInKeplerianElements;
    asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3;
    asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
    asterixInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
    asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
            = unit_conversions::convertDegreesToRadians( 235.7 );
    asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
            = unit_conversions::convertDegreesToRadians( 23.4 );
    asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

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

    // Set (perturbed) initial state.
    Eigen::Matrix< StateScalarType, 6, 1 > initialTranslationalState = convertKeplerianToCartesianElements(
                asterixInitialStateInKeplerianElements, earthGravitationalParameter );
    initialTranslationalState += initialStateDifference;

    // Create propagator settings
    std::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< StateScalarType > >
            ( centralBodies, accelerationModelMap, bodiesToIntegrate, initialTranslationalState,
              TimeType( finalEphemerisTime ), cowell );

    // Define parameters.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    {
        parameterNames.push_back(
                    std::make_shared< InitialTranslationalStateEstimatableParameterSettings< StateScalarType > >(
                        "Vehicle", initialTranslationalState, "Earth" ) );

        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< EstimatableParameterSettings >( "Moon", gravitational_parameter ) );

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

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

    // Perturb parameters.
    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > parameterVector =
            parametersToEstimate->template getFullParameterValues< StateScalarType >( );
    parameterVector.block( 6, 0, numberOfParametersToEstimate, 1 ) += parameterPerturbation;
    parametersToEstimate->resetParameterValues( parameterVector );

    std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
            std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > results;

    {
        // Create dynamics simulator
        SingleArcVariationalEquationsSolver< StateScalarType, TimeType > dynamicsSimulator =
                SingleArcVariationalEquationsSolver< StateScalarType, TimeType >(
                    bodyMap, integratorSettings, propagatorSettings, parametersToEstimate,
                    1, std::shared_ptr< numerical_integrators::IntegratorSettings< double > >( ), 0, 0 );

        // Propagate requested equations.
        if( propagateVariationalEquations )
        {
            dynamicsSimulator.integrateVariationalAndDynamicalEquations( propagatorSettings->getInitialStates( ), 1 );
        }
        else
        {
            dynamicsSimulator.integrateDynamicalEquationsOfMotionOnly( propagatorSettings->getInitialStates( ) );
        }

        // Retrieve test data
        double testEpoch = initialEphemerisTime + 3.5 * 3600.0;
        Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > testStates =
                Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( 12 );
        testStates.block( 0, 0, 6, 1 ) = bodyMap[ "Vehicle" ]->getEphemeris( )->getCartesianState( testEpoch );

        if( propagateVariationalEquations )
        {
            results.first.push_back( dynamicsSimulator.getStateTransitionMatrixInterface( )->
                                     getCombinedStateTransitionAndSensitivityMatrix( testEpoch ) );
        }
        results.second.push_back( testStates );
    }
    return results;
}

//! Test the state transition and sensitivity matrix computation against their numerical propagation for Earth orbiter.
/*!
 *  Test the state transition and sensitivity matrix computation against their numerical propagation for Earth orbiter.
 *  This unit test propagates the variational equations for a low Earth orbiter, using the Enckle propagator.
 *  The results are compared against results obtained from numerical differentiation (first-order central difference).
 *  The estimated parameters consist of a mass of a third-body, radiation pressure coefficient and drag coefficient of the
 *  vehicle and spherical harmonic coefficients of the Earth
 */
BOOST_AUTO_TEST_CASE( testEarthOrbiterVariationalEquationCalculation )
{
    std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput;

    // Define variables for numerical differentiation
    Eigen::Matrix< double, 6, 1 > perturbedState;
    Eigen::Matrix< double, 6, 1 > statePerturbation;

    // Define parameter perturbation
    int numberOfParametersToEstimate = 10;
    double sphericalHarmonicsPerturbation = 1.0E-6;
    Eigen::Matrix< double, 10, 1 > perturbedParameter;
    Eigen::Matrix< double, 10, 1 > parameterPerturbation  =
            ( Eigen::Matrix< double, 10, 1 >( ) << 10.0, 1.0, 1.0E12,
              sphericalHarmonicsPerturbation, sphericalHarmonicsPerturbation,
              sphericalHarmonicsPerturbation, sphericalHarmonicsPerturbation,
              sphericalHarmonicsPerturbation, sphericalHarmonicsPerturbation, sphericalHarmonicsPerturbation ).finished( );

    // Define state perturbation
    statePerturbation = ( Eigen::Matrix< double, 6, 1>( ) <<
                          10.0, 10.0, 10.0, 0.01, 0.01, 0.01 ).finished( );

    // Compute state transition and sensitivity matrices
    currentOutput = executeOrbiterSimulation< double, double >(
                Eigen::Matrix< double, 6, 1 >::Zero( ) );
    Eigen::MatrixXd stateTransitionAndSensitivityMatrixAtEpoch = currentOutput.first.at( 0 );
    Eigen::MatrixXd manualPartial = Eigen::MatrixXd::Zero( 6, 6 + numberOfParametersToEstimate );

    // Numerically compute state transition matrix
    for( unsigned int j = 0; j < 6; j++ )
    {
        Eigen::VectorXd upPerturbedState, downPerturbedState;
        perturbedState.setZero( );
        perturbedState( j ) += statePerturbation( j );
        upPerturbedState = executeOrbiterSimulation< double, double >(
                    perturbedState, Eigen::Matrix< double, 10, 1 >::Zero( ), 0 ).second.at( 0 );

        perturbedState.setZero( );
        perturbedState( j ) -= statePerturbation( j );
        downPerturbedState = executeOrbiterSimulation< double, double >(
                    perturbedState, Eigen::Matrix< double, 10, 1 >::Zero( ), 0 ).second.at( 0 );

        manualPartial.block( 0, j, 6, 1 ) =
                ( upPerturbedState.segment( 0, 6 ) - downPerturbedState.segment( 0, 6 ) ) / ( 2.0 * statePerturbation( j ) );
    }

    // Numerically compute sensitivity matrix
    for( int j = 0; j < numberOfParametersToEstimate; j ++ )
    {
        Eigen::VectorXd upPerturbedState, downPerturbedState;
        perturbedState.setZero( );
        Eigen::Matrix< double, 10, 1 > upPerturbedParameter, downPerturbedParameter;
        perturbedParameter.setZero( );
        perturbedParameter( j ) += parameterPerturbation( j );

        upPerturbedState = executeOrbiterSimulation< double, double >(
                    perturbedState, perturbedParameter ).second.at( 0 );

        perturbedParameter.setZero( );
        perturbedParameter( j ) -= parameterPerturbation( j );
        downPerturbedState = executeOrbiterSimulation< double, double >(
                    perturbedState, perturbedParameter ).second.at( 0 );

        manualPartial.block( 0, j + 6, 6, 1 ) =
                ( upPerturbedState.segment( 0, 6 ) - downPerturbedState.segment( 0, 6 ) ) / ( 2.0 * parameterPerturbation( j ) );
    }

    // Check results
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                stateTransitionAndSensitivityMatrixAtEpoch, manualPartial, 5.0E-5 );

}

template< typename TimeType = double , typename StateScalarType  = double >
        std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > >
executePhobosRotationSimulation(
        const Eigen::Matrix< StateScalarType, 13, 1 > initialStateDifference,
        Eigen::Matrix< StateScalarType, 13, 1 >& appliedStateDifference,
        const Eigen::VectorXd parameterPerturbation = Eigen::VectorXd::Zero( 8 ),
        const bool propagateVariationalEquations = 1 )
{
    double initialEphemerisTime = 0.0;
    double finalEphemerisTime = 86400.0;
    int numberOfParametersToEstimate = 8;

    NamedBodyMap bodyMap;
    bodyMap[ "Mars" ] = std::make_shared< Body >( );
    bodyMap[ "Mars" ]->setEphemeris( std::make_shared< ephemerides::ConstantEphemeris >(
                                         [ = ]( ){ return Eigen::Vector6d::Zero( ); } ) );
    bodyMap[ "Mars" ]->setRotationalEphemeris(
                simulation_setup::createRotationModel( simulation_setup::getDefaultRotationModelSettings(
                                                           "Mars", initialEphemerisTime, finalEphemerisTime ), "Mars" ) );
    bodyMap[ "Mars" ]->setGravityFieldModel(
                simulation_setup::createGravityFieldModel(
                    simulation_setup::getDefaultGravityFieldSettings(
                        "Mars", initialEphemerisTime, finalEphemerisTime ), "Mars", bodyMap ) );
    double marsGravitationalParameter = bodyMap.at( "Mars" )->getGravityFieldModel( )->getGravitationalParameter( );

    bodyMap[ "Phobos" ] = std::make_shared< Body >( );

    Eigen::Matrix3d phobosInertiaTensor = Eigen::Matrix3d::Zero( );
    phobosInertiaTensor( 0, 0 ) = 0.3615;
    phobosInertiaTensor( 1, 1 ) = 0.4265;
    phobosInertiaTensor( 2, 2 ) = 0.5024;

    phobosInertiaTensor *= ( 11.27E3 * 11.27E3 * 1.0659E16 );
    bodyMap[ "Phobos" ]->setBodyInertiaTensor(
                phobosInertiaTensor, ( 0.3615 + 0.4265 + 0.5024 ) / 3.0 );

    double phobosGravitationalParameter = 1.0659E16 * physical_constants::GRAVITATIONAL_CONSTANT;
    double phobosReferenceRadius = 11.27E3;

    Eigen::MatrixXd phobosCosineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 ),
            phobosSineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 );
    double phobosScaledMeanMomentOfInertia;
    gravitation::getDegreeTwoSphericalHarmonicCoefficients(
                phobosInertiaTensor, phobosGravitationalParameter, phobosReferenceRadius, true,
                phobosCosineGravityFieldCoefficients, phobosSineGravityFieldCoefficients, phobosScaledMeanMomentOfInertia );

    bodyMap[ "Phobos" ]->setGravityFieldModel(
                std::make_shared< gravitation::SphericalHarmonicsGravityField >(
                    phobosGravitationalParameter, phobosReferenceRadius, phobosCosineGravityFieldCoefficients,
                    phobosSineGravityFieldCoefficients, "Phobos_Fixed",
                    std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( "Phobos" ), true ) ) );

    Eigen::Vector6d phobosKeplerElements = Eigen::Vector6d::Zero( );
    double phobosSemiMajorAxis = 9376.0E3;
    phobosKeplerElements( 0 ) = phobosSemiMajorAxis;
    phobosKeplerElements( 2 ) = 0.1;

    bodyMap[ "Phobos" ]->setEphemeris(
                tudat::ephemerides::getTabulatedEphemeris( std::make_shared< ephemerides::KeplerEphemeris >(
                                                               phobosKeplerElements, 0.0, marsGravitationalParameter ),
                                                           initialEphemerisTime - 3600.0,
                                                           finalEphemerisTime + 3600, 60.0 ) );

    Eigen::Quaterniond noRotationQuaternion = Eigen::Quaterniond( Eigen::AngleAxisd( 1.0E-0, Eigen::Vector3d::UnitZ( ) ) *
                                                                  Eigen::AngleAxisd( 2.0E-0, Eigen::Vector3d::UnitX( ) ) *
                                                                  Eigen::AngleAxisd( -0.5E-0, Eigen::Vector3d::UnitZ( ) ) );
    Eigen::Matrix< double, 7, 1 > unitRotationState = Eigen::Matrix< double, 7, 1 >::Zero( );
    unitRotationState( 0 ) = noRotationQuaternion.w( );
    unitRotationState( 1 ) = noRotationQuaternion.x( );
    unitRotationState( 2 ) = noRotationQuaternion.y( );
    unitRotationState( 3 ) = noRotationQuaternion.z( );
    unitRotationState( 4 ) = 1.0E-4;
    unitRotationState( 5 ) = -1.0E-4;
    unitRotationState( 6 ) = std::sqrt( marsGravitationalParameter /
                                        std::pow( phobosSemiMajorAxis, 3.0 ) ) + 1.0E-4;

    Eigen::Matrix< double, 7, 1 > originalRotationState = unitRotationState;
    Eigen::Matrix< double, 7, 1 > stateDifferenceToAdd = initialStateDifference.segment( 6, 7 );

    unitRotationState += stateDifferenceToAdd;
    unitRotationState( 0 ) = originalRotationState( 0 ) / std::fabs( originalRotationState( 0 ) ) *
            std::sqrt( 1.0 - std::pow( unitRotationState.segment( 1, 3 ).norm( ), 2.0 ) );

    appliedStateDifference.segment( 0, 6 ) = initialStateDifference.segment( 0, 6 );
    appliedStateDifference.segment( 6, 7 ) = unitRotationState - originalRotationState ;
    std::map< double, Eigen::Matrix< double, 7, 1 > > dummyRotationMap;
    dummyRotationMap[ -1.0E100 ] = unitRotationState;
    dummyRotationMap[ 1.0E100 ] = unitRotationState;

    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 7, 1 > > > dummyInterpolator =
            std::make_shared< interpolators::LinearInterpolator< double, Eigen::Matrix< double, 7, 1 > > >( dummyRotationMap );
    bodyMap[ "Phobos" ]->setRotationalEphemeris( std::make_shared< TabulatedRotationalEphemeris< double, double > >(
                                                     dummyInterpolator, "ECLIPJ2000", "Phobos_Fixed" ) );

    // Create empty bodies, phobos and mars.
    std::shared_ptr< Body > phobos = bodyMap.at( "Phobos" );
    std::shared_ptr< Body > mars = bodyMap.at( "Mars" );
    setGlobalFrameBodyEphemerides( bodyMap, "Mars", "ECLIPJ2000" );

    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfEarth;
//    accelerationMap[ "Phobos" ][ "Mars" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
   accelerationMap[ "Phobos" ][ "Mars" ].push_back( std::make_shared< MutualSphericalHarmonicAccelerationSettings >( 2, 2, 2, 2 ) );

    std::vector< std::string > translationalBodiesToIntegrate;
    std::vector< std::string > translationalCentralBodies;

    translationalBodiesToIntegrate.push_back( "Phobos" );
    translationalCentralBodies.push_back( "Mars" );

    AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodyMap, accelerationMap, translationalBodiesToIntegrate, translationalCentralBodies );

    SelectedTorqueMap torqueMap;
    torqueMap[ "Phobos" ][ "Mars" ].push_back(
                std::make_shared< TorqueSettings >( second_order_gravitational_torque ) );

    // Define propagator settings.
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Phobos" );

    // Create torque models
    basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                bodyMap, torqueMap, bodiesToIntegrate );

    std::shared_ptr< RotationalStatePropagatorSettings< double > > rotationalPropagatorSettings =
            std::make_shared< RotationalStatePropagatorSettings< double > >
            ( torqueModelMap, bodiesToIntegrate, unitRotationState, std::make_shared< PropagationTimeTerminationSettings >(
                  finalEphemerisTime ) );

    Eigen::VectorXd initialTranslationalState;
    initialTranslationalState = getInitialStatesOfBodies(
                translationalBodiesToIntegrate, translationalCentralBodies, bodyMap, initialEphemerisTime );

    initialTranslationalState += initialStateDifference.segment( 0, 6 );
    std::shared_ptr< TranslationalStatePropagatorSettings< > > translationalPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< > >
            ( translationalCentralBodies, accelerationModelMap, translationalBodiesToIntegrate, initialTranslationalState,
              finalEphemerisTime, cowell );

    std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > >  propagatorSettingsList;
    propagatorSettingsList.push_back( rotationalPropagatorSettings );
    propagatorSettingsList.push_back( translationalPropagatorSettings );

    std::shared_ptr< SingleArcPropagatorSettings< double > > propagatorSettings = std::make_shared< MultiTypePropagatorSettings< double > >(
                propagatorSettingsList,
                std::make_shared< PropagationTimeTerminationSettings >( finalEphemerisTime ) );

    // Create integrator settings
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings =
            std::make_shared< IntegratorSettings< TimeType > >
            ( rungeKutta4, TimeType( initialEphemerisTime ), 15.0 );

    // Define parameters.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    {
        parameterNames.push_back( std::make_shared< InitialRotationalStateEstimatableParameterSettings< double > >(
                                      "Phobos", unitRotationState, "ECLIPJ2000" ) );
        parameterNames.push_back( std::make_shared< InitialTranslationalStateEstimatableParameterSettings< double > >(
                                      "Phobos", initialTranslationalState, "Mars" ) );

        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Phobos", mean_moment_of_inertia ) );
        parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                      1, 0, 2, 2, "Phobos", spherical_harmonics_cosine_coefficient_block ) );
        parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                      2, 1, 2, 2, "Phobos", spherical_harmonics_sine_coefficient_block ) );
    }

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

    Eigen::MatrixXd constraintStateMultiplier;
    Eigen::VectorXd constraintRightHandSide;
    parametersToEstimate->getConstraints( constraintStateMultiplier, constraintRightHandSide );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( constraintStateMultiplier.block( 0, 0, 1, 4 ) ), ( unitRotationState.segment( 0, 4 ) ).transpose( ),
                std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( constraintStateMultiplier.block( 0, 4, 1, 9 ) ), ( Eigen::MatrixXd::Zero( 1, 9 ) ),
                std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( constraintRightHandSide.block( 0, 0, 1, 1 ) ), ( Eigen::MatrixXd::Zero( 1, 1 ) ),
                std::numeric_limits< double >::epsilon( ) );

    // Perturb parameters.
    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > parameterVector =
            parametersToEstimate->template getFullParameterValues< StateScalarType >( );
    parameterVector.block( 13, 0, numberOfParametersToEstimate, 1 ) += parameterPerturbation;
//    std::cout<<"Parameter perturbation "<<
//               ( parametersToEstimate->template getFullParameterValues< StateScalarType >( ) -
//                 parameterVector ).transpose( )<<std::endl<<
//               parameterPerturbation.transpose( )<<std::endl;;
    parametersToEstimate->resetParameterValues( parameterVector );

    std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
            std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > results;

    {
        // Create dynamics simulator
        propagators::SingleArcVariationalEquationsSolver< StateScalarType, TimeType > dynamicsSimulator =
                propagators::SingleArcVariationalEquationsSolver< StateScalarType, TimeType >(
                    bodyMap, integratorSettings, propagatorSettings, parametersToEstimate,
                    1, std::shared_ptr< numerical_integrators::IntegratorSettings< double > >( ), 0, 0 );

        // Propagate requested equations.
        if( propagateVariationalEquations )
        {
            dynamicsSimulator.integrateVariationalAndDynamicalEquations( propagatorSettings->getInitialStates( ), 1 );
        }
        else
        {
            dynamicsSimulator.integrateDynamicalEquationsOfMotionOnly( propagatorSettings->getInitialStates( ) );
        }

        //        tudat::input_output::writeDataMapToTextFile(
        //                    dynamicsSimulator.getDynamicsSimulator( )->getEquationsOfMotionNumericalSolution( ),
        //                    "rotPropTest.dat" );

        // Retrieve test data
        double testEpoch = initialEphemerisTime + 15.0 * 3600.0;
        Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > testStates =
                Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( 13 );
        testStates.segment( 6, 7 ) = bodyMap[ "Phobos" ]->getRotationalEphemeris( )->getRotationStateVector( testEpoch );
        testStates.segment( 0, 6 ) = bodyMap[ "Phobos" ]->getEphemeris( )->getCartesianState( testEpoch );

        if( propagateVariationalEquations )
        {
            results.first.push_back( dynamicsSimulator.getStateTransitionMatrixInterface( )->
                                     getCombinedStateTransitionAndSensitivityMatrix( testEpoch ) );

        }
        results.second.push_back( testStates );

    }
    return results;
}

BOOST_AUTO_TEST_CASE( testPhobosRotationVariationalEquationCalculation )
{
    spice_interface::loadStandardSpiceKernels( );

    std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput;

    // Define variables for numerical differentiation
    Eigen::Matrix< double, 13, 1 > perturbedState;
    Eigen::Matrix< double, 13, 1 > statePerturbation;

    // Define parameter perturbation
    int numberOfParametersToEstimate = 8;
    double sphericalHarmonicsPerturbation = 1.0E-4;
    Eigen::Matrix< double, 8, 1 > perturbedParameter;
    Eigen::Matrix< double, 8, 1 > parameterPerturbation;
    parameterPerturbation = Eigen::Matrix< double, 8, 1 >::Constant( sphericalHarmonicsPerturbation );
    parameterPerturbation( 0 ) = 1.0E-4;

    // Compute state transition and sensitivity matrices
    Eigen::Matrix< double, 13, 1 > appliedStateDifference;
    currentOutput = executePhobosRotationSimulation< double, double >(
                Eigen::Matrix< double, 13, 1 >::Zero( ), appliedStateDifference );
    Eigen::MatrixXd stateTransitionAndSensitivityMatrixAtEpoch = currentOutput.first.at( 0 );
    Eigen::VectorXd nominalState = currentOutput.second.at( 0 );

    // Define state perturbation
    statePerturbation = ( Eigen::Matrix< double, 13, 1>( ) <<
                          10.0, 10.0, 10.0, 0.1, 0.01, 0.01,
                          1.0E-5, 1.0E-5, 1.0E-5, 1.0E-5, 1.0E-7, 1.0E-7, 1.0E-7 ).finished( );

    Eigen::MatrixXd manualPartial = Eigen::MatrixXd::Zero( 13, 13 + numberOfParametersToEstimate );

    // Numerically compute state transition matrix
    for( unsigned int test = 0; test < 2; test++ )
    {
        double perturbationMultiplier = ( test == 0 ? 1.0 : 1.0E-3 );
        for( unsigned int j = 7; j < 10; j++ )
        {
            Eigen::Matrix< double, 13, 1 > appliedStateDifferenceUp, appliedStateDifferenceDown;

            Eigen::VectorXd upPerturbedState, downPerturbedState;
            perturbedState.setZero( );
            perturbedState( j ) += perturbationMultiplier * statePerturbation( j );
            upPerturbedState = executePhobosRotationSimulation< double, double >(
                        perturbedState, appliedStateDifferenceUp, Eigen::Matrix< double, 8, 1 >::Zero( ), 0 ).second.at( 0 );

            Eigen::VectorXd stateDifferenceUp = upPerturbedState - nominalState;

            std::cout<<"Test output "<<test<<" "<<j<<"stateDifferenceUp"<<std::endl<<
                       stateDifferenceUp<<std::endl<<std::endl<<
                       "stateTransitionAndSensitivityMatrixAtEpoch"<<std::endl<<
                                              stateTransitionAndSensitivityMatrixAtEpoch<<std::endl<<std::endl<<
                       "appliedStateDifferenceUp"<<std::endl<<
                                              appliedStateDifferenceUp<<std::endl<<std::endl<<
                       "( stateTransitionAndSensitivityMatrixAtEpoch * appliedStateDifferenceUp )"<<std::endl<<
                                              ( stateTransitionAndSensitivityMatrixAtEpoch * appliedStateDifferenceUp )<<std::endl<<std::endl;
            if( test == 0 )
            {
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            ( ( stateTransitionAndSensitivityMatrixAtEpoch.block( 0, 0, 13, 13 ) * appliedStateDifferenceUp ).segment( 0, 6 ) ),
                            ( stateDifferenceUp.segment( 0, 6 ) ), 1.0E-3 );
            }
            else
            {
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            ( ( stateTransitionAndSensitivityMatrixAtEpoch.block( 0, 0, 13, 13 ) * appliedStateDifferenceUp ).segment( 6, 7 ) ),
                            ( stateDifferenceUp.segment( 6, 7 ) ), 1.0E-5 );
            }
        }
    }

    for( unsigned int j = 0; j < 6; j++ )
    {
        Eigen::Matrix< double, 13, 1 > appliedStateDifferenceUp, appliedStateDifferenceDown;

        Eigen::VectorXd upPerturbedState, downPerturbedState;
        perturbedState.setZero( );
        perturbedState( j ) += statePerturbation( j );
        upPerturbedState = executePhobosRotationSimulation< double, double >(
                    perturbedState, appliedStateDifferenceUp, Eigen::Matrix< double, 8, 1 >::Zero( ), 0 ).second.at( 0 );

        perturbedState.setZero( );
        perturbedState( j ) -= statePerturbation( j );
        downPerturbedState = executePhobosRotationSimulation< double, double >(
                    perturbedState, appliedStateDifferenceDown, Eigen::Matrix< double, 8, 1 >::Zero( ), 0 ).second.at( 0 );

        manualPartial.block( 0, j, 13, 1 ) =
                ( upPerturbedState.segment( 0, 13 ) - downPerturbedState.segment( 0, 13 ) ) / ( 2.0 * statePerturbation( j ) );

    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( manualPartial.block( 0, 0, 13, 6 ) ), ( stateTransitionAndSensitivityMatrixAtEpoch.block( 0, 0, 13, 6 ) ), 1.0E-4 );

    for( unsigned int j = 10; j < 13; j++ )
    {
        Eigen::Matrix< double, 13, 1 > appliedStateDifferenceUp, appliedStateDifferenceDown;

        Eigen::VectorXd upPerturbedState, downPerturbedState;
        perturbedState.setZero( );
        perturbedState( j ) += statePerturbation( j );
        upPerturbedState = executePhobosRotationSimulation< double, double >(
                    perturbedState, appliedStateDifferenceUp, Eigen::Matrix< double, 8, 1 >::Zero( ), 0 ).second.at( 0 );

        perturbedState.setZero( );
        perturbedState( j ) -= statePerturbation( j );
        downPerturbedState = executePhobosRotationSimulation< double, double >(
                    perturbedState, appliedStateDifferenceDown, Eigen::Matrix< double, 8, 1 >::Zero( ), 0 ).second.at( 0 );

        manualPartial.block( 0, j, 13, 1 ) =
                ( upPerturbedState.segment( 0, 13 ) - downPerturbedState.segment( 0, 13 ) ) / ( 2.0 * statePerturbation( j ) );

    }

    // Check element separately
    BOOST_CHECK_SMALL( std::fabs( manualPartial( 3, 1 + 10 ) -
                                  stateTransitionAndSensitivityMatrixAtEpoch( 3, 1 + 10 ) ), 1.0E-2 );
    manualPartial( 3, 1 + 10 ) = stateTransitionAndSensitivityMatrixAtEpoch( 3, 1 + 10 );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( manualPartial.block( 0, 10, 6, 3 ) ), ( stateTransitionAndSensitivityMatrixAtEpoch.block( 0, 10, 6, 3 ) ), 1.0E-3 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( manualPartial.block( 6, 10, 7, 3 ) ), ( stateTransitionAndSensitivityMatrixAtEpoch.block( 6, 10, 7, 3 ) ), 1.0E-5 );

    // Numerically compute sensitivity matrix
    for( int j = 0; j < numberOfParametersToEstimate; j ++ )
    {
        Eigen::Matrix< double, 13, 1 > appliedStateDifference;

        Eigen::VectorXd upPerturbedState, downPerturbedState;
        perturbedState.setZero( );
        perturbedParameter.setZero( );
        perturbedParameter( j ) += parameterPerturbation( j );

        std::cout<<"Test "<<j<<" "<<perturbedParameter.transpose( )<<std::endl;

        upPerturbedState = executePhobosRotationSimulation< double, double >(
                    perturbedState, appliedStateDifference, perturbedParameter, 0 ).second.at( 0 );

        perturbedParameter.setZero( );
        perturbedParameter( j ) -= parameterPerturbation( j );
        downPerturbedState = executePhobosRotationSimulation< double, double >(
                    perturbedState, appliedStateDifference, perturbedParameter, 0 ).second.at( 0 );

        manualPartial.block( 0, j + 13, 13, 1 ) =
                ( upPerturbedState.segment( 0, 13 ) - downPerturbedState.segment( 0, 13 ) ) / ( 2.0 * parameterPerturbation( j ) );
    }
    std::cout<<manualPartial<<std::endl<<std::endl
            <<stateTransitionAndSensitivityMatrixAtEpoch<<std::endl<<std::endl<<
              ( manualPartial - stateTransitionAndSensitivityMatrixAtEpoch ).cwiseQuotient(
                  stateTransitionAndSensitivityMatrixAtEpoch )<<std::endl;

    // Check three values separately: could not find perturbations for which all partials are sufficiently within the linear regime

    BOOST_CHECK_SMALL( std::fabs( manualPartial( 4, 5 + 13 ) -
                                  stateTransitionAndSensitivityMatrixAtEpoch( 4, 5 + 13 ) ), 1.0E-4 );
    BOOST_CHECK_SMALL( std::fabs( manualPartial( 11, 4 + 13 ) -
                                  stateTransitionAndSensitivityMatrixAtEpoch( 11, 4 + 13 ) ), 1.0E-5 );
    BOOST_CHECK_SMALL( std::fabs( manualPartial( 12, 3 + 13 ) -
                                  stateTransitionAndSensitivityMatrixAtEpoch( 12, 3 + 13 ) ), 1.0E-2 );

    manualPartial( 4, 5 + 13 ) =  stateTransitionAndSensitivityMatrixAtEpoch( 4, 5 + 13 );
    manualPartial( 11, 4 + 13 ) =  stateTransitionAndSensitivityMatrixAtEpoch( 11, 4 + 13 );
    manualPartial( 12, 3 + 13 ) =  stateTransitionAndSensitivityMatrixAtEpoch( 12, 3 + 13 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( manualPartial.block( 0, 13, 13, 8 ) ), ( stateTransitionAndSensitivityMatrixAtEpoch.block( 0, 13, 13, 8 ) ), 1.0E-4 );
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


// Boost.Geometry Index
//
// Spatial index distance predicates, calculators and checkers
// used in nearest query - specialized for envelopes
//
// Copyright (c) 2011-2015 Adam Wulkiewicz, Lodz, Poland.
//
// This file was modified by Oracle on 2019.
// Modifications copyright (c) 2019 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_DISTANCE_PREDICATES_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_DISTANCE_PREDICATES_HPP

#include <boost/geometry/index/detail/algorithms/comparable_distance_near.hpp>
#include <boost/geometry/index/detail/algorithms/comparable_distance_far.hpp>
#include <boost/geometry/index/detail/algorithms/comparable_distance_centroid.hpp>
#include <boost/geometry/index/detail/algorithms/path_intersection.hpp>

#include <boost/geometry/index/detail/tags.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

// ------------------------------------------------------------------ //
// relations
// ------------------------------------------------------------------ //

template <typename T>
struct to_nearest
{
    to_nearest(T const& v) : value(v) {}
    T value;
};

template <typename T>
struct to_centroid
{
    to_centroid(T const& v) : value(v) {}
    T value;
};

template <typename T>
struct to_furthest
{
    to_furthest(T const& v) : value(v) {}
    T value;
};

// tags

struct to_nearest_tag {};
struct to_centroid_tag {};
struct to_furthest_tag {};

// ------------------------------------------------------------------ //
// relation traits and access
// ------------------------------------------------------------------ //

template <typename T>
struct relation
{
    typedef T value_type;
    typedef to_nearest_tag tag;
    static inline T const& value(T const& v) { return v; }
    static inline T & value(T & v) { return v; }
};

template <typename T>
struct relation< to_nearest<T> >
{
    typedef T value_type;
    typedef to_nearest_tag tag;
    static inline T const& value(to_nearest<T> const& r) { return r.value; }
    static inline T & value(to_nearest<T> & r) { return r.value; }
};

template <typename T>
struct relation< to_centroid<T> >
{
    typedef T value_type;
    typedef to_centroid_tag tag;
    static inline T const& value(to_centroid<T> const& r) { return r.value; }
    static inline T & value(to_centroid<T> & r) { return r.value; }
};

template <typename T>
struct relation< to_furthest<T> >
{
    typedef T value_type;
    typedef to_furthest_tag tag;
    static inline T const& value(to_furthest<T> const& r) { return r.value; }
    static inline T & value(to_furthest<T> & r) { return r.value; }
};

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

template
<
    typename G1, typename G2, typename Strategy,
    typename Tag1 = typename geometry::tag<G1>::type,
    typename Tag2 = typename geometry::tag<G2>::type
>
struct comparable_distance_call_base
{
    typedef typename geometry::default_comparable_distance_result
        <
            G1, G2
        >::type result_type;

    static inline result_type apply(G1 const& g1, G2 const& g2, Strategy const&)
    {
        return geometry::comparable_distance(g1, g2);
    }
};

template
<
    typename G1, typename G2, typename Strategy
>
struct comparable_distance_call_base<G1, G2, Strategy, point_tag, point_tag>
{
    typedef typename geometry::comparable_distance_result
        <
            G1, G2,
            typename Strategy::comparable_distance_point_point_strategy_type
        >::type result_type;

    static inline result_type apply(G1 const& g1, G2 const& g2, Strategy const& s)
    {
        return geometry::comparable_distance(g1, g2,
                s.get_comparable_distance_point_point_strategy());
    }
};

template
<
    typename G1, typename G2, typename Strategy
>
struct comparable_distance_call_base<G1, G2, Strategy, point_tag, box_tag>
{
    typedef typename geometry::comparable_distance_result
        <
            G1, G2,
            typename Strategy::comparable_distance_point_box_strategy_type
        >::type result_type;

    static inline result_type apply(G1 const& g1, G2 const& g2, Strategy const& s)
    {
        return geometry::comparable_distance(g1, g2,
                s.get_comparable_distance_point_box_strategy());
    }
};

template
<
    typename G1, typename G2, typename Strategy
>
struct comparable_distance_call_base<G1, G2, Strategy, segment_tag, point_tag>
{
    typedef typename geometry::comparable_distance_result
        <
            G1, G2,
            typename Strategy::comparable_distance_point_segment_strategy_type
        >::type result_type;

    static inline result_type apply(G1 const& g1, G2 const& g2, Strategy const& s)
    {
        return geometry::comparable_distance(g1, g2,
                s.get_comparable_distance_point_segment_strategy());
    }
};

template
<
    typename G1, typename G2, typename Strategy
>
struct comparable_distance_call_base<G1, G2, Strategy, segment_tag, box_tag>
{
    typedef typename geometry::comparable_distance_result
        <
            G1, G2,
            typename Strategy::comparable_distance_segment_box_strategy_type
        >::type result_type;

    static inline result_type apply(G1 const& g1, G2 const& g2, Strategy const& s)
    {
        return geometry::comparable_distance(g1, g2,
                s.get_comparable_distance_segment_box_strategy());
    }
};

template
<
    typename G1, typename G2, typename Strategy
>
struct comparable_distance_call_base<G1, G2, Strategy, segment_tag, segment_tag>
{
    typedef typename geometry::comparable_distance_result
        <
            G1, G2,
            typename Strategy::comparable_distance_point_segment_strategy_type
        >::type result_type;

    static inline result_type apply(G1 const& g1, G2 const& g2, Strategy const& s)
    {
        return geometry::comparable_distance(g1, g2,
                s.get_comparable_distance_point_segment_strategy());
    }
};

template
<
    typename G1, typename G2, typename Strategy
>
struct comparable_distance_call
    : comparable_distance_call_base<G1, G2, Strategy>
{};

template
<
    typename G1, typename G2
>
struct comparable_distance_call<G1, G2, default_strategy>
    : comparable_distance_call_base<G1, G2, default_strategy, void, void>
{};

// ------------------------------------------------------------------ //
// calculate_distance
// ------------------------------------------------------------------ //

template <typename Predicate, typename Indexable, typename Strategy, typename Tag>
struct calculate_distance
{
    BOOST_MPL_ASSERT_MSG((false), INVALID_PREDICATE_OR_TAG, (calculate_distance));
};

// this handles nearest() with default Point parameter, to_nearest() and bounds
template <typename PointRelation, typename Indexable, typename Strategy, typename Tag>
struct calculate_distance< predicates::nearest<PointRelation>, Indexable, Strategy, Tag>
{
    typedef detail::relation<PointRelation> relation;
    typedef comparable_distance_call
        <
            typename relation::value_type,
            Indexable,
            Strategy
        > call_type;
    typedef typename call_type::result_type result_type;

    static inline bool apply(predicates::nearest<PointRelation> const& p, Indexable const& i,
                             Strategy const& s, result_type & result)
    {
        result = call_type::apply(relation::value(p.point_or_relation), i, s);
        return true;
    }
};

template <typename Point, typename Indexable, typename Strategy>
struct calculate_distance< predicates::nearest< to_centroid<Point> >, Indexable, Strategy, value_tag>
{
    typedef Point point_type;
    typedef typename geometry::default_comparable_distance_result
        <
            point_type, Indexable
        >::type result_type;

    static inline bool apply(predicates::nearest< to_centroid<Point> > const& p, Indexable const& i,
                             Strategy const& , result_type & result)
    {
        result = index::detail::comparable_distance_centroid(p.point_or_relation.value, i);
        return true;
    }
};

template <typename Point, typename Indexable, typename Strategy>
struct calculate_distance< predicates::nearest< to_furthest<Point> >, Indexable, Strategy, value_tag>
{
    typedef Point point_type;
    typedef typename geometry::default_comparable_distance_result
        <
            point_type, Indexable
        >::type result_type;

    static inline bool apply(predicates::nearest< to_furthest<Point> > const& p, Indexable const& i,
                             Strategy const& , result_type & result)
    {
        result = index::detail::comparable_distance_far(p.point_or_relation.value, i);
        return true;
    }
};

template <typename SegmentOrLinestring, typename Indexable, typename Strategy, typename Tag>
struct calculate_distance< predicates::path<SegmentOrLinestring>, Indexable, Strategy, Tag>
{
    typedef typename index::detail::default_path_intersection_distance_type<
        Indexable, SegmentOrLinestring
    >::type result_type;

    static inline bool apply(predicates::path<SegmentOrLinestring> const& p, Indexable const& i,
                             Strategy const& , result_type & result)
    {
        return index::detail::path_intersection(i, p.geometry, result);
    }
};

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_INDEX_RTREE_DISTANCE_PREDICATES_HPP


#ifndef MOLPPCORE_HPP
#define MOLPPCORE_HPP

#include <Eigen/Dense>

using Point3 = Eigen::Vector3f;
using Coord3 = Eigen::Matrix3Xf;

#endif // MOLPPCORE_HPP


/**
   @file map.hpp
   @author Kevin Hu
   @date 2009.11.25
 */
#ifndef MAP_HPP
#define MAP_HPP

#include <types.h>
#include <am/am.h>
#include <am/concept/DataType.h>

#include <util/hashFunction.h>
#include <boost/static_assert.hpp>

#include <cstdlib>
#include <cstddef>
#include <fstream>
#include <istream>
#include <iostream>

#ifdef _DEBUG
#include "debug_new.h"
#endif

using namespace std;

using namespace izenelib::am::util;

class simple_hash
{
public:
    static uint32_t getValue(const char* p, size_t len)
    {
        return izenelib::util::sdb_hash_fun(p,len);
    }

    static uint32_t getValue(const unsigned int& key, size_t x=0)
    {
        return key;

    }

};

NS_IZENELIB_AM_BEGIN
/**
   @class Map
 *@brief This is an implementation of open addressing and linear probing hash table for integer key.
 *
 **/
template<
class KeyType = uint32_t,
      class ValueType = uint64_t,
      size_t ENTRY_POW = 16,//!< entry size will be 2^ENTRY_POW
      class HASH_FUNCTION = simple_hash,
      int INIT_MAP_BUCKET_SIZE=64//!< initial size of bucket
      >
class Map : public AccessMethod<KeyType, ValueType>
{
    enum {ENTRY_SIZE = (2<<ENTRY_POW)};
    enum {ENTRY_MASK = ENTRY_SIZE-1};
    enum {ENTRY_GROW = 5};

    typedef izenelib::am::DataType<KeyType,ValueType> DataType;
    typedef Map<KeyType, ValueType, ENTRY_POW, HASH_FUNCTION> SelfType;

    typedef boost::archive::binary_iarchive iarchive;
    typedef boost::archive::binary_oarchive oarchive;

    struct KEY_
    {
        KeyType k_;
        inline KEY_()
        {
            k_ = (KeyType)-1;
        }

        inline KEY_(const KeyType& k)
        {
            k_ = k;
        }

        bool operator != (const KeyType& k)const
        {
            return k_!=k;
        }

        bool operator == (const KeyType& k)const
        {
            return k_==k;
        }

    };

    struct ENTRY_
    {
        KEY_* pK_;
        size_t*  pIdx_;
    };

public:
    Map():entry_size_(ENTRY_SIZE)
    {
        entry_.pK_ = (KEY_*) malloc( entry_size_ * sizeof(KEY_) );
        entry_.pIdx_ = (size_t*) malloc( entry_size_ * sizeof(size_t) );
        count_ = 0;
    }

    ~Map()
    {
        free(entry_.pK_);
        free(entry_.pIdx_);
        count_ = 0;
        dataVec_.clear();
    }

    void clear()
    {
        free(entry_.pK_);
        free(entry_.pIdx_);
        count_ = 0;
        dataVec_.clear();
    }

    bool insert(const DataType& data)
    {
        return insert(data.get_key(), data.get_value());
    }

    bool update(const DataType& dat)
    {
        return update(dat.get_key(), dat.get_value());
    }

    int num_items() const
    {
        return count_;
    }

    void display(std::ostream & os=std::cout)
    {
    }

    bool insert(const KeyType& key, const ValueType& v)
    {
        KeyType idx = key & ENTRY_MASK;

        while (idx<entry_size_ && entry_.pK_[idx] != (KeyType)-1)
        {
            if (entry_.pK_[idx] == key)
                return false;

            idx ++;
        }

        if (idx == entry_size_)
        {
            entry_.pK_ = (KEY_*)realloc(entry_.pK_, (entry_size_+ENTRY_GROW)*sizeof(KEY_));

            for (KeyType i=0; i<ENTRY_GROW; i++)
                entry_.pK_[entry_size_+i] = (KeyType)-1;

            entry_.pIdx_ =  (size_t*)realloc((void*)entry_.pIdx_, (entry_size_+ENTRY_GROW)*sizeof(size_t));

            entry_size_ += ENTRY_GROW;
        }

        entry_.pK_[idx] = key;
        entry_.pIdx_[idx] = dataVec_.size();
        dataVec_.push_back(v);
        count_++;

        return true;
    }

    bool get(const KeyType&key, ValueType& value)
    {
        ValueType* ret = find(key);
        if(!ret)
        {
            value = *ret;
            return true;
        }
        else return false;
    }

    ValueType* find(const KeyType& key)
    {
        KeyType idx = key & ENTRY_MASK;

        while (idx<entry_size_)
        {
            if (entry_.pK_[idx] == (KeyType)-1)
                return NULL;

            if (entry_.pK_[idx] == key)
            {
                return &dataVec_[entry_.pIdx_[idx]];
            }

            idx ++;
        }

        return NULL;
    }

    bool del(const KeyType& key)
    {

        KeyType idx = key & ENTRY_MASK;

        while (idx<entry_size_)
        {
            if (entry_.pK_[idx] == (KeyType)-1)
                return false;

            if (entry_.pK_[idx] == key)
            {
                entry_.pK_[idx] = (KeyType)-1;
                count_--;
                return true;
            }

            idx ++;
        }

        return false;
    }

    bool update(const KeyType& key, const ValueType& v)
    {
        KeyType idx = key & ENTRY_MASK;

        while (idx<entry_size_)
        {
            if (entry_.pK_[idx] == (KeyType)-1)
                return false;

            if (entry_.pK_[idx] == key)
            {
                entry_.pIdx_[idx] = dataVec_.size();
                dataVec_.push_back(v);
                return true;
            }

            idx ++;
        }

        return true;
    }

    friend class boost::serialization::access;
    template<class Archive>
    void save(Archive & ar, const unsigned int version)  const
    {
        ar & entry_size_;
        ar.save_binary(entry_.pK_, entry_size_*sizeof(KEY_));
        ar.save_binary(entry_.pIdx_, entry_size_*sizeof(size_t));
        ar & count_;
        ar & dataVec_;
    }

    template<class Archive>
    void load(Archive & ar, const unsigned int version)
    {
        clear();

        ar & entry_size_;
        entry_.pK_ = (KEY_*)malloc(entry_size_*sizeof(KEY_));
        entry_.pIdx_ = (size_t*)malloc(entry_size_*sizeof(size_t));

        ar.load_binary(entry_.pK_, entry_size_*sizeof(KEY_));
        ar.load_binary(entry_.pIdx_, entry_size_*sizeof(size_t));

        ar & count_;
        ar & dataVec_;
    }

    BOOST_SERIALIZATION_SPLIT_MEMBER()

    void save(const string& file)  const
    {
        ofstream of(file.c_str(), ios_base::trunc);
        oarchive oa(of);
        oa<<*this;
        of.close();
    }

    void load(const string& file)
    {
        ifstream of(file.c_str());
        iarchive ia(of);
        ia >> *this;
        of.close();
    }

protected:
    ENTRY_ entry_;
    vector<ValueType> dataVec_;
    KeyType entry_size_;
    int count_;

};

//*****************************************************************************
/**
 * @brief cccr_hash stands for Cache-Conscious Collision Resolution String Hash Table.
 *
 *  This is based on work of Nikolas Askitis and Justin Zobel, 'Cache-Conscious Collision Resolution
 *     in String Hash Tables' .On typical current machines each cache miss incurs a delay of
 *  hundreds of clock cycles while data is fetched from memory. This approach both
 *  saves space and eliminates a potential cache miss at each node access, at little cost.
 *  In experiments with large sets of strings drawn from real-world data, we show that,
 *  in comparison to standard chaining, compact-chain hash tables can yield both space
 *  savings and reductions in per-string access times.
 *
 *
 **/

#define PAGE_EXPANDING 1
#define EXACT_EXPANDING 0

template<
class ValueType,
      size_t ENTRY_POW,
      class HASH_FUNCTION,
      int INIT_MAP_BUCKET_SIZE
      >
class Map<string, ValueType, ENTRY_POW, HASH_FUNCTION, INIT_MAP_BUCKET_SIZE>
    : public AccessMethod<string, ValueType>
{
    enum {ENTRY_SIZE = (2<<ENTRY_POW)};
    enum {ENTRY_MASK = ENTRY_SIZE-1};
    enum {EXPAND = PAGE_EXPANDING};

    typedef izenelib::am::DataType<string,ValueType> DataType;
    typedef Map<string, ValueType, ENTRY_POW, HASH_FUNCTION> SelfType;

    typedef boost::archive::binary_iarchive iarchive;
    typedef boost::archive::binary_oarchive oarchive;

public:
    Map()
    {
        for(size_t i=0; i<ENTRY_SIZE; i++)
        {
            entry_[i] = NULL;
        }
        count_ = 0;
    }

    ~Map()
    {

        for(size_t i=0; i<ENTRY_SIZE; i++)
        {
            if (entry_[i]!=NULL) delete[] entry_[i];
            entry_[i] = NULL;
        }

    }

    void clear()
    {

        for(size_t i=0; i<ENTRY_SIZE; i++)
        {
            if (entry_[i]!=NULL) delete[] entry_[i];
            entry_[i] = NULL;
        }
        dataVec_.clear();
        count_ = 0;
    }

    bool insert(const DataType& data)
    {
        return insert(data.get_key(), data.get_value());
    }

    bool update(const DataType& dat)
    {
        return update(dat.get_key(), dat.get_value());
    }

    int num_items() const
    {
        return count_;
    }

    void display(std::ostream & os=std::cout)
    {
        for(size_t i=0; i<ENTRY_SIZE; i++)
        {
            char* pBkt = entry_[i];

            if (pBkt ==NULL)
                continue;

            os<<endl<<i<<" Entry===========\n";

            uint32_t bs = *(uint32_t*)(pBkt);
            uint32_t content_len = *((uint32_t*)(pBkt)+1);
            os<<"bucket size: "<<bs<<endl;
            os<<"content len: "<<content_len<<endl;

            uint32_t p = sizeof(uint32_t)*2;;
            while (p < content_len)
            {
                size_t len = *((size_t*)(pBkt+p));
                p += sizeof (size_t);
                os<<"("<<len<<")";

                for (size_t j=0; j<len; j++)
                    os<<pBkt[p+j];
                p += len;

                os<<"=>"<<*((uint64_t*)(pBkt+p))<<endl;
                p += sizeof (uint64_t);
            }
        }
    }

    bool insert(const string& str, const ValueType& v)
    {
        size_t ksize = str.length();

        uint32_t idx = HASH_FUNCTION::getValue(str.c_str(),ksize) & ENTRY_MASK;

        uint64_t value = dataVec_.size();

        dataVec_.push_back(v);

        char* pBkt = entry_[idx];

        if (pBkt == NULL)
        {
            entry_[idx] = pBkt = new char[INIT_MAP_BUCKET_SIZE];
            *(uint32_t*)(pBkt) = 64;
            *((uint32_t*)(pBkt)+1) = 2*sizeof(uint32_t);
            good_entries_.push_back(idx);
        }
        else
        {
            //cout<<"does it exist?";
            uint32_t content_len = *((uint32_t*)pBkt+1);
            uint32_t p = sizeof(uint32_t)*2;;
            while (p < content_len)
            {
                size_t len = *((size_t*)(pBkt+p));

                if (len != ksize )
                {
                    p += sizeof (size_t) + len + sizeof(uint64_t);
                    continue;
                }

                p += sizeof (size_t);

                size_t j=0;
                for (; j<len; j++)
                    if (str[j] != pBkt[p+j])
                        break;

                if (j == len)
                {
                    *(uint64_t*)(pBkt+p+j) = value;
                    return true;
                }

                p += len + sizeof (uint64_t);
            }
        }

        //cout<<"==============\n";
        uint32_t bs = *(uint32_t*)(pBkt);
        uint32_t content_len = *((uint32_t*)pBkt+1);

        if (bs-content_len<ksize+sizeof(size_t)+sizeof(uint64_t))
        {
            bs += EXPAND ==PAGE_EXPANDING? INIT_MAP_BUCKET_SIZE: ksize+sizeof(size_t)+sizeof(uint64_t);
            //content_len += str.length()+sizeof(uint32_t);
            pBkt = new char[bs];
            memcpy(pBkt, entry_[idx], *(uint32_t*)(entry_[idx]) );
            delete[] entry_[idx];
            entry_[idx] = pBkt;
        }

        *((size_t*)(pBkt+content_len)) = ksize;
        content_len += sizeof(size_t);
        memcpy(pBkt+content_len, str.c_str(), ksize);
        content_len += ksize;
        *((uint64_t*)(pBkt+content_len)) = value;
        content_len += sizeof(uint64_t);

        *(uint32_t*)(pBkt) = bs;
        *((uint32_t*)pBkt+1) = content_len;

        count_++;
        return true;

    }

    bool get(const string&key, ValueType& value)
    {
        ValueType* ret = find(key);
        if(!ret)
        {
            value = *ret;
            return true;
        }
        else return false;
    }

    ValueType* find(const string& str)
    {
        size_t ksize = str.length();

        uint32_t idx = HASH_FUNCTION::getValue(str.c_str(),ksize) & ENTRY_MASK;

        char* pBkt = entry_[idx];
        if (pBkt == NULL)
            return NULL;

        uint32_t content_len = *((uint32_t*)(pBkt)+1);

        uint32_t p = sizeof(uint32_t)*2;;
        while (p < content_len)
        {
            size_t len = *((size_t*)(pBkt+p));
            p += sizeof (size_t);
            if (ksize != len)
            {
                p += len + sizeof (uint64_t);
                continue;
            }

            size_t i = 0;
            for (; i<len; i++)
            {
                if (pBkt[p+i]!=str[i])
                    break;
            }
            if (i != len)
            {
                p += len + sizeof (uint64_t);
                continue;
            }

            p += len;

            if (*((uint64_t*)(pBkt+p))!=(uint64_t)-1)
                return &(dataVec_[*((uint64_t*)(pBkt+p))]);
            else
                return NULL;

        }

        return NULL;

    }

    bool del(const string& str)
    {
        size_t ksize = str.length();

        uint32_t idx = HASH_FUNCTION::getValue(str.c_str(),ksize) & ENTRY_MASK;

        char* pBkt = entry_[idx];
        if (pBkt ==NULL)
            return -1;

        uint32_t content_len = *((uint32_t*)(pBkt)+1);

        uint32_t p = sizeof(uint32_t)*2;;
        while (p < content_len)
        {
            size_t len = *((size_t*)(pBkt+p));
            p += sizeof (size_t);
            if (ksize != len)
            {
                p += len + sizeof (uint64_t);
                continue;
            }

            size_t i = 0;
            for (; i<len; i++)
            {
                if (pBkt[p+i]!=str[i])
                    break;
            }
            if (i != len)
            {
                p += len + sizeof (uint64_t);
                continue;
            }

            p += len;

            *((uint64_t*)(pBkt+p)) = (uint64_t)-1;

            count_ --;
            return true;
        }

        return false;
    }

    bool update(const string& str, const ValueType& v)
    {
        size_t ksize = str.length();
        uint32_t idx = HASH_FUNCTION::getValue(str.c_str(),ksize) & ENTRY_MASK;

        char* pBkt = entry_[idx];

        if (pBkt ==NULL)
            return -1;

        uint32_t content_len = *((uint32_t*)(pBkt)+1);

        uint32_t p = sizeof(uint32_t)*2;;
        while (p < content_len)
        {
            size_t len = *((size_t*)(pBkt+p));
            p += sizeof (size_t);
            if (ksize != len)
            {
                p += len + sizeof (uint64_t);
                continue;
            }

            size_t i = 0;

            for (; i<len; i++)
            {
                if (pBkt[p+i]!=str[i])
                    break;
            }
            if (i != len)
            {
                p += len + sizeof (uint64_t);
                continue;
            }

            p += len;

            if (*((uint64_t*)(pBkt+p)) != (uint64_t)-1)
            {
                dataVec_[*((uint64_t*)(pBkt+p))] = v;
                return true;
            }
            else
                return false;
        }

        return false;
    }

    friend class boost::serialization::access;
    template<class Archive>
    void save(Archive & ar, const unsigned int version)  const
    {
        ar & good_entries_;

        for (uint64_t i=0; i<good_entries_.size(); i++)
        {
            char* pBkt = entry_[good_entries_[i]];

            uint32_t bs = *(uint32_t*)(pBkt);

            ar & bs;
            ar.save_binary(pBkt, bs);
        }

        ar & count_ & dataVec_;
    }

    template<class Archive>
    void load(Archive & ar, const unsigned int version)
    {

        ar & good_entries_;

        for (uint64_t i=0; i<good_entries_.size(); i++)
        {
            uint32_t bs;
            ar & bs;

            entry_[good_entries_[i]] = new char[bs];
            ar.load_binary(entry_[good_entries_[i]], bs);

        }

        ar & count_  & dataVec_;
    }

    BOOST_SERIALIZATION_SPLIT_MEMBER()

    void save(const string& file)  const
    {
        ofstream of(file.c_str(), ios_base::out|ios_base::trunc);

        oarchive oa(of);
        oa<<*this;
        of.close();
    }

    void load(const string& file)
    {
        ifstream of(file.c_str());
        iarchive ia(of);
        ia >> *this;
        of.close();
    }

protected:
    char* entry_[ENTRY_SIZE];
    vector<ValueType> dataVec_;
    vector<uint64_t> good_entries_;
    int count_;

};

NS_IZENELIB_AM_END

#endif