///////////////////////////////////////////////////////////////////////////////
/// \file integrate.hpp
/// Like \c std::accumulate for the whole series, multiplied by the discretization
//
//  Copyright 2006 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TIME_SERIES_NUMERIC_INTEGRATE_EAN_06_14_2006
#define BOOST_TIME_SERIES_NUMERIC_INTEGRATE_EAN_06_14_2006

#include <boost/implicit_cast.hpp>
#include <boost/concept/requires.hpp>
#include <boost/range_run_storage/algorithm/copy.hpp>
#include <boost/time_series/concepts.hpp>
#include <boost/time_series/utility/time_series_base.hpp>

namespace boost { namespace time_series
{
    namespace detail
    {
        template<typename Value>
        struct integrate_inserter
        {
            explicit integrate_inserter(Value const &value)
              : value_(value)
            {}

            template<typename Run>
            void set_at(Run const &run, Value const &value)
            {
                this->value_ += value * range_run_storage::length(run);
            }

            Value const &value() const
            {
                return this->value_;
            }
        private:
            Value value_;
        };
    }

    /// \brief Calculates the integral of the series.
    ///
    /// The integral is calculated by multiplying each run's value by the run's length,
    ///     summing all the results, and multiplying the sum by the series' discretization.
    ///
    /// \param series The input series.
    /// \pre Given a variable \c x of the series' \c value_type and \c y of the series'
    ///     \c length_type, the expression \c (x*y) must be convertible to \c value_type.
    /// \pre Given variables \c x and \c y of the series' \c value_type, the expression
    ///     \c (x+y) must be convertible to \c value_type.
    /// \pre Given a variable \c x of the series' \c value_type and \c y of the series'
    ///     \c discretization_type, the expression \c (x*y) must be convertible to \c value_type.
    /// \return A \c value_type holding the result of \c std::accumulate of the series,
    ///     multiplied by the series discretization.
    /// \attention Currently assumes the zeros of the series do not participate
    ///     in the integral.
    template<typename Series>
    BOOST_CONCEPT_REQUIRES(
        ((concepts::TimeSeries<Series const>)),
    (typename concepts::TimeSeries<Series const>::value_type))
    integrate(Series const &series)
    {
        typedef typename concepts::TimeSeries<Series const>::value_type value_type;

        detail::integrate_inserter<value_type> o(
            implicit_cast<value_type const &>(range_run_storage::zero(series)));
        range_run_storage::copy(series, o);
        return o.value() * series.discretization();
    }

}}

#endif // BOOST_TIME_SERIES_NUMERIC_INTEGRATE_EAN_06_14_2006


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/constant/log2_e.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check log2_e behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log2_e;
  using boost::simd::Log2_e;
  T ref = T(1);
  STF_TYPE_IS(decltype(Log2_e<T>()), T);
  STF_EQUAL(Log2_e<T>(), ref);
  STF_EQUAL(log2_e( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log2_e behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log2_e;
  using boost::simd::Log2_e;
  T ref = T(1.442695040888963407359924681);

  STF_TYPE_IS(decltype(Log2_e<T>()), T);
  STF_IEEE_EQUAL(Log2_e<T>(), ref);
  STF_IEEE_EQUAL(log2_e( as(T{}) ), ref);
}

STF_CASE_TPL( "Check log2_e behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log2_e;
  using boost::simd::Log2_e;
  T ref = T(1.442695040888963407359924681);
  STF_TYPE_IS(decltype(Log2_e<T>()), T);
  STF_IEEE_EQUAL(Log2_e<T>(), ref);
  STF_IEEE_EQUAL(log2_e( as(T{}) ), ref);
}


#pragma once

#include <Eigen/Core>

namespace ipc {

double suggest_good_voxel_size(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    double inflation_radius = 0);

double suggest_good_voxel_size(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    double inflation_radius = 0);

/// @brief Compute the average edge length of a mesh.
double average_edge_length(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E);

/// @brief Compute the average displacement length.
double average_displacement_length(const Eigen::MatrixXd& U);

} // namespace ipc


// Boost.Maral library (Molecular Archiving, Retrieval & Algorithm Library)
//
// Copyright (C) 2014-2015 Armin Madadkar Sobhani
//
// Distributed under the Boost Software License Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// $Id$

#ifndef MARAL_ALGORITHMS_RMSD_HPP
#define MARAL_ALGORITHMS_RMSD_HPP

#include <vector>

//#include <boost/foreach.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/distance.hpp>

namespace maral {

template<class AtomRange>
inline typename pntvec_traits<decltype(
    std::declval<std::remove_pointer
    <typename AtomRange::value_type>::type>().get_center())>::value_type
rmsd(
    const AtomRange& rng1
,   const AtomRange& rng2)
{
    typedef typename pntvec_traits<decltype(
        std::declval<std::remove_pointer
        <typename AtomRange::value_type>::type>().get_center())>::value_type
        return_type;

    auto n = boost::distance(rng1);
    BOOST_ASSERT_MSG(n == boost::distance(rng2),
        "Size of two ranges must be equal!");

    return_type sum = return_type(0);

    //auto atm1 = *boost::begin(rng1);
    //auto atm2 = *boost::begin(rng2);
    //BOOST_FOREACH(boost::tie(atm1, atm2), boost::combine(rng1, rng2))
    //    sum += distance2(atm1->center(), atm2->center());

    for (auto tup : boost::combine(rng1, rng2))
    {
        typename AtomRange::value_type atm1, atm2;
        boost::tie(atm1, atm2) = tup;
        sum += distance2(atm1->center(), atm2->center());
    }

    return sqrt(sum/n);
}

}   // maral

#endif    // MARAL_ALGORITHMS_RMSD_HPP


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

  @copyright 2017 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_ALGORITHM_MIN_ELEMENT_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_MIN_ELEMENT_HPP_INCLUDED

#include <boost/simd/algorithm/find.hpp>
#include <boost/simd/algorithm/min_val.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns an iterator pointing to the element with the smallest value in the range [first,last).

    @param first  Beginning of the range of elements to min_element
    @param last   End of the range of elements to min_element
    @param comp   comparison function object that will be applied.

    @par Requirement

      - @c first and @c last must be pointer to Vectorizable type.

      - @c comp must be a polymorphic unary function object, i.e callable on generic types.
      - if @c comp is not present the function test is done with operator <

    @par Example:

      @snippet min_element.cpp min_element

    @par Possible output:

      @snippet min_element.txt min_element

  **/
  template<typename T, typename Comp>
  T const * min_element(T const* first, T const* last, Comp comp)
  {
    if (first == last) return last;
    return find(first, last, min_val(first,last,comp));
  }

  template<typename T> T const * min_element(T const* first, T const* last)
  {
    if (first == last) return last;
    return find(first, last, min_val(first,last));
  }
} }

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Ganeff, M.I. Solar radiation pressure benchmark data script,
 *          solarRadiationPressureBenchmarkData.m, available at http://tudat.tudelft.nl, 2012.
 *      Giancoli, D.C. Physics for Scientists and Engineers with Modern Physics
 *          Fourth Edition. New Jersey: Prentice-Hall, Inc., 1985.
 *      Hirsh, S.M. Solar radiation pressure benchmark data script,
 *          solarRadiationUnitTests.cpp, available at
 *          https://github.com/sethhirsh/solarRadiationUnitTests, 2013.
 *      Irizarry, V. The effect of solar radiation pressure on Ulysses orbit,
 *          http://ccar.colorado.edu/asen5050/projects/projects_2001/aponte/Ulysses.htm, 2001,
 *          last accessed: 7th October, 2013.
 *      Willmott, P. General astro Library, http://www.amsat-bda.org/GAL_Downloads.html,
 *          2011, last accessed: 7th October, 2013.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

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

#include <Eigen/Core>

#include "tudat/basics/testMacros.h"

#include "tudat/astro/electromagnetism/solarSailForce.h"
#include "tudat/astro/electromagnetism/solarSailAcceleration.h"
#include "tudat/astro/electromagnetism/cannonBallRadiationPressureForce.h"
#include "tudat/astro/electromagnetism/cannonBallRadiationPressureAcceleration.h"
#include "tudat/astro/basic_astro/unitConversions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_solar_sail_acceleration_and_force_models )

// Set radiation pressure at 1 AU [N/m^2].
const double radiationPressureAtOneAU = 4.56e-6;

// Set 1 AU in metres [m].
const double astronomicalUnitInMeters = 1.49598e11;

// Set gravitational parameter of the Earth [m^3 s^-2].
// Reference: Mass taken from Table 1.2, pg. 6, (de Pater, 2010), value
//            of gravitational constant taken from http://ssd.jpl.nasa.gov/?constants#ref.
const double earthGravitationalParameter = 3.9859383624e14;

//! Test solar sail force in the case where the non-ideal solar sail model is equivalent to the cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailModelVsCannonBall )
{
    // Data for the cannon ball model.

    // Set radiation pressure coefficient (1 + emissivity).
    const double radiationPressureCoefficient = 1.0 + 0.21;

    // Set area on target that is subject to radiation pressure [m^2].
    const double areaSubjectToRadiationPressure = 0.5;

    // Set position vector [m].
    Eigen::Vector3d positionVectorToSource
            = Eigen::Vector3d( astronomicalUnitInMeters, astronomicalUnitInMeters, 0.0 );

    // Compute distance between spacecraft and source [m].
    const double positionVectorToSourceNorm = positionVectorToSource.norm();

    // Set radiation pressure at target [N/m^2].
    const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

    // Normalize position vector from spacecraft to source [-].
    positionVectorToSource.normalize( );

    // Compute radiation pressure force from cannonball radiation pressure model [N].
    const Eigen::Vector3d computedRadiationPressureForce = electromagnetism::computeCannonBallRadiationPressureForce(
                radiationPressureAtTarget, positionVectorToSource, areaSubjectToRadiationPressure, radiationPressureCoefficient );

    // Define solar sail model settings.
    const double frontEmissivityCoefficient = 0.4;
    const double backEmissivityCoefficient = frontEmissivityCoefficient;
    const double frontLambertianCoefficient = 0.4;
    const double backLambertianCoefficient = 0.4;
    const double reflectivityCoefficient = 0.21;
    const double specularReflectionCoefficient = 1.0;
    const double coneAngle = 0.0;
    const double clockAngle = 0.0;

    // Compute velocity vector of the spacecraft w.r.t. central body [m/s].
    Eigen::Vector3d velocityVector = std::sqrt( earthGravitationalParameter / positionVectorToSourceNorm )
        * Eigen::Vector3d( std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 );

    // Compute force from solar sail model [N].
    const Eigen::Vector3d computedSolarSailForce = electromagnetism::computeSolarSailForce(
                frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient, backLambertianCoefficient,
                reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource, velocityVector.normalized(),
                radiationPressureAtTarget, areaSubjectToRadiationPressure, coneAngle, clockAngle);

    // Compare computed and expected radiation pressure force vectors.
    BOOST_CHECK_EQUAL( computedRadiationPressureForce.z( ), computedSolarSailForce.z( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRadiationPressureForce.segment( 0, 2 ), computedSolarSailForce.segment( 0, 2 ),
                                       1.0e-15 );
}

//! Test solar sail acceleration model on spacecraft at approximately 1 AU away from the Sun, with a solar sail model
//! equivalent to the cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelEarth )
{
    // Benchmark data is obtained using the General astro Library (Willmott, 2011).

    // Set radiation pressure coefficient (1 + emissivity).
    const double radiationPressureCoefficient = 1.0 + 0.3;

    // Set area on target that is subject to radiation pressure [m^2].
    const double areaSubjectToRadiationPressure = 2.0;

    // Set position vector [m].
    Eigen::Vector3d positionVectorToSource = Eigen::Vector3d( astronomicalUnitInMeters, 0.0, 0.0 );

    // Compute distance between spacecraft and source [m].
    const double positionVectorToSourceNorm = positionVectorToSource.norm();

    // Set radiation pressure at target [N/m^2].
    const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

    // Normalize position vector to get vector pointing to source in non-dimensional units [-].
    positionVectorToSource.normalize( );

    // Set mass of accelerated body [kg].
    const double mass = 4.0;

    // Compute radiation pressure acceleration from cannonball radiation pressure model [m/s^2].
    const Eigen::Vector3d computedRadiationPressureAcceleration = electromagnetism::computeCannonBallRadiationPressureAcceleration(
                radiationPressureAtTarget, positionVectorToSource, areaSubjectToRadiationPressure, radiationPressureCoefficient, mass );

    // Define solar sail model settings.
    const double frontEmissivityCoefficient = 0.4;
    const double backEmissivityCoefficient = frontEmissivityCoefficient;
    const double frontLambertianCoefficient = 0.4;
    const double backLambertianCoefficient = 0.4;
    const double reflectivityCoefficient = 0.3;
    const double specularReflectionCoefficient = 1.0;
    const double coneAngle = 0.0;
    const double clockAngle = 0.0;

    // Compute velocity vector of the spacecraft w.r.t. central body [m/s].
    Eigen::Vector3d velocityVector = std::sqrt( earthGravitationalParameter / positionVectorToSourceNorm )
            * Eigen::Vector3d( std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 );

    // Compute solar sail acceleration [m/s^2].
    const Eigen::Vector3d computedSolarSailAcceleration = electromagnetism::computeSolarSailAcceleration(
                frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient, backLambertianCoefficient,
                reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource, velocityVector.normalized(),
                radiationPressureAtTarget, areaSubjectToRadiationPressure, coneAngle, clockAngle, mass);

    // Compare computed and expected radiation pressure acceleration vectors
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRadiationPressureAcceleration, computedSolarSailAcceleration,
                                       1.0e-15 );
}

//! Test solar sail acceleration model on spacecraft at approximately the distance of Venus away from the Sun, with a solar sail model
//! equivalent to the cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelVenus )
{
    // Benchmark data is obtained using the General astro Library (Willmott, 2011).

    // Set the distance from the Sun to Venus [m].
    const double distanceSunToVenus = 0.732 * astronomicalUnitInMeters;

    // Set radiation pressure coefficient (1 + emissivity).
    const double radiationPressureCoefficient = 1.0 + 0.5;

    // Set area on target that is subject to radiation pressure [m^2].
    const double areaSubjectToRadiationPressure = 0.005;

    // Set position vector [m].
    Eigen::Vector3d positionVectorToSource
            = Eigen::Vector3d( distanceSunToVenus / std::sqrt( 2.0 ), distanceSunToVenus / std::sqrt( 2.0 ), 0.0 );

    // Set radiation pressure at target [N/m^2].
    const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

    // Normalize position vector to get vector pointing to source in non-dimensional units [-].
    positionVectorToSource.normalize( );

    // Set mass of accelerated body [kg].
    const double mass = 0.0022;

    // Compute radiation pressure acceleration from cannonball radiation pressure model [m/s^2].
    const Eigen::Vector3d computedRadiationPressureAcceleration = electromagnetism::computeCannonBallRadiationPressureAcceleration(
                radiationPressureAtTarget, positionVectorToSource, areaSubjectToRadiationPressure, radiationPressureCoefficient, mass );

    // Define solar sail model settings.
    const double frontEmissivityCoefficient = 0.4;
    const double backEmissivityCoefficient = frontEmissivityCoefficient;
    const double frontLambertianCoefficient = 0.4;
    const double backLambertianCoefficient = 0.4;
    const double reflectivityCoefficient = 0.5;
    const double specularReflectionCoefficient = 1.0;
    const double coneAngle = 0.0;
    const double clockAngle = 0.0;

    // Compute velocity vector of the spacecraft w.r.t. central body [m/s].
    Eigen::Vector3d velocityVector = std::sqrt( earthGravitationalParameter / distanceSunToVenus)
        * Eigen::Vector3d( std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 );

    // Compute solar sail acceleration [m/s^2].
    const Eigen::Vector3d computedSolarSailAcceleration
        = electromagnetism::computeSolarSailAcceleration(
            frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient,
            backLambertianCoefficient, reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource,
            velocityVector.normalized(), radiationPressureAtTarget, areaSubjectToRadiationPressure, coneAngle, clockAngle, mass);

    // Check there is no acceleration generated along the z-axis.
    BOOST_CHECK_SMALL( computedRadiationPressureAcceleration.z( ), std::numeric_limits< double >::min( ) );

    // Compare computed and expected radiation pressure acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRadiationPressureAcceleration.segment( 0, 2 ),
                                       computedSolarSailAcceleration.segment( 0, 2 ),
                                       1.0e-14 );
}

//! Test solar sail force model on spacecraft at approximately the distance of Uranus away from the Sun, with a solar sail model
//! equivalent to the cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailForceModelUranus )
{
    // Benchmark data is obtained using the General astro Library (Willmott, 2011).

    // Set radiation pressure coefficient (1 + emissivity).
    const double radiationPressureCoefficient = 1.0 + 0.8;

    // Set area on target that is subject to radiation pressure [m^2].
    const double areaSubjectToRadiationPressure = 69939064094327.4;

    // Define distance between Uranus and the Sun, which is also the distance between the spacecraft and the Sun [m].
    const double distanceSunToUranus = 9.529 * astronomicalUnitInMeters;

    // Set position vector [m].
    Eigen::Vector3d positionVectorToSource
            = Eigen::Vector3d( distanceSunToUranus / std::sqrt( 2.0 ), distanceSunToUranus / std::sqrt( 2.0 ), 0.0 );

    // Compute radiation pressure at target [N/m^2].
    const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

    // Normalize position vector to get vector pointing to source in non-dimensional units [-].
    positionVectorToSource.normalize( );

    // Compute radiation pressure force from cannonball radiation pressure model [N].
    const Eigen::Vector3d computedRadiationPressureForce = electromagnetism::computeCannonBallRadiationPressureForce(
                radiationPressureAtTarget, positionVectorToSource, areaSubjectToRadiationPressure, radiationPressureCoefficient );

    // Define solar sail model settings.
    const double frontEmissivityCoefficient = 0.4;
    const double backEmissivityCoefficient = frontEmissivityCoefficient;
    const double frontLambertianCoefficient = 0.4;
    const double backLambertianCoefficient = 0.4;
    const double reflectivityCoefficient = 0.8;
    const double specularReflectionCoefficient = 1.0;
    const double coneAngle = 0.0;
    const double clockAngle = 0.0;

    // Compute velocity vector of the spacecraft w.r.t. the central body [m/s].
    Eigen::Vector3d velocityVector = std::sqrt( earthGravitationalParameter / distanceSunToUranus )
        * Eigen::Vector3d( std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 );

    // Compute force from the solar sail model [N].
    const Eigen::Vector3d computedSolarSailForce = electromagnetism::computeSolarSailForce(
                frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient,
                backLambertianCoefficient, reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource,
                velocityVector.normalized(), radiationPressureAtTarget, areaSubjectToRadiationPressure, coneAngle, clockAngle);

    // Compare computed and expected radiation pressure force vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRadiationPressureForce,
                                       computedSolarSailForce,
                                       1.0e-14 );
}

//! Test solar sail force model on a spacecraft at a random distance from the Sun, with a solar sail model equivalent to the
//! cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailForceModelRandom )
{
    // Benchmark data is obtained using the General astro Library (Willmott, 2011).

    // Set radiation pressure coefficient (1 + emissivity).
    const double radiationPressureCoefficient = 1.0 + 0.4058;

    // Set area on target that is subject to radiation pressure [m^2].
    const double areaSubjectToRadiationPressure = 514701.9505;

    // Set position vector [m].
    Eigen::Vector3d positionVectorToSource = Eigen::Vector3d( 94359740.25, 90831886.1, 14668782.92 );

    // Compute distance of the spacecraft w.r.t. the Sun [m].
    const double positionVectorToSourceNorm = positionVectorToSource.norm();

    // Set radiation pressure at target [N/m^2].
    const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

    // Normalize position vector to get vector pointing to source in non-dimensional units [-].
    positionVectorToSource.normalize( );

    // Compute radiation pressure force from cannonball radiation pressure model [N].
    const Eigen::Vector3d computedRadiationPressureForce = electromagnetism::computeCannonBallRadiationPressureForce(
                radiationPressureAtTarget, positionVectorToSource,
                areaSubjectToRadiationPressure, radiationPressureCoefficient );

    // Define solar sail model settings.
    const double frontEmissivityCoefficient = 0.4;
    const double backEmissivityCoefficient = frontEmissivityCoefficient;
    const double frontLambertianCoefficient = 0.4;
    const double backLambertianCoefficient = 0.4;
    const double reflectivityCoefficient = 0.4058;
    const double specularReflectionCoefficient = 1.0;
    const double coneAngle = 0.0;
    const double clockAngle = 0.0;

    // Compute velocity vector of the spacecraft w.r.t. the central body [m/s].
    Eigen::Vector3d velocityVector = std::sqrt( earthGravitationalParameter / positionVectorToSourceNorm )
        * Eigen::Vector3d( std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 );

    // Compute force from solar sail model [N].
    const Eigen::Vector3d computedSolarSailForce = electromagnetism::computeSolarSailForce(
            frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient,
            backLambertianCoefficient, reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource,
            velocityVector.normalized(), radiationPressureAtTarget, areaSubjectToRadiationPressure, coneAngle, clockAngle);

    // Compare computed and expected radiation pressure force vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRadiationPressureForce, computedSolarSailForce, 1.0e-14 );
}

//! Test radiation force model on a hand at approximately 1 AU from the Sun, with a solar sail model equivalent to the
//! cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailForceModelGiancoliData )
{
    // Benchmark data is obtained from Physics for Scientists and Engineers
    // with Modern Physics Volume 2 (Ch. 31, Ex. 7) (Giancoli, 1985).

    // Set radiation pressure coefficient (1 + emissivity).
    const double radiationPressureCoefficient = 1.0 + 0.0;

    // Set area on target that is subject to radiation pressure [m^2].
    const double areaSubjectToRadiationPressure = 0.02;

    // Set position vector [m].
    Eigen::Vector3d positionVectorToSource = Eigen::Vector3d( astronomicalUnitInMeters, 0.0, 0.0 );

    // Set radiation pressure at target [N/m^2].
    const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

    // Normalize position vector to get vector pointing to source in non-dimensional units [-].
    positionVectorToSource.normalize( );

    // Compute radiation pressure force from cannonball radiation pressure model [N].
    const Eigen::Vector3d computedRadiationPressureForce = electromagnetism::computeCannonBallRadiationPressureForce(
                radiationPressureAtTarget, positionVectorToSource, areaSubjectToRadiationPressure, radiationPressureCoefficient );

    // Define solar sail settings.
    const double frontEmissivityCoefficient = 0.4;
    const double backEmissivityCoefficient = frontEmissivityCoefficient;
    const double frontLambertianCoefficient = 0.4;
    const double backLambertianCoefficient = 0.4;
    const double reflectivityCoefficient = 0.0;
    const double specularReflectionCoefficient = 1.0;
    const double coneAngle = 0.0;
    const double clockAngle = 0.0;

    // Compute velocity vector of the spacecraft w.r.t. the central body [m/s].
    Eigen::Vector3d velocityVector = std::sqrt(earthGravitationalParameter/astronomicalUnitInMeters)
        * Eigen::Vector3d( std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 );

    // Compute force from solar sail model [N].
    const Eigen::Vector3d computedSolarSailForce = electromagnetism::computeSolarSailForce(
                frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient,
                backLambertianCoefficient, reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource,
                velocityVector.normalized(), radiationPressureAtTarget, areaSubjectToRadiationPressure, coneAngle, clockAngle);

    // Check there is no force component along the x-axis.
    BOOST_CHECK_SMALL( std::fabs( computedRadiationPressureForce.x( ) - computedSolarSailForce.x( ) ), 1.0e-6 );

    // Compare computed and expected radiation pressure force vectors.
    BOOST_CHECK_SMALL( computedSolarSailForce.y( ), std::numeric_limits< double >::min( ) );
    BOOST_CHECK_SMALL( computedSolarSailForce.z( ), std::numeric_limits< double >::min( ) );
}

//! Test radiation acceleration model on Ulysses satellite at 1AU, with a solar sail model equivalent to the
//! cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelUlysses )
{
    // Benchmark data obtained from (Irizarry, 2001).

    // Set radiation pressure coefficient (1 + emissivity).
    const double radiationPressureCoefficient = 1.0 + 0.327;

    // Set area on target that is subject to radiation pressure [m^2].
    const double areaSubjectToRadiationPressure = 10.59;

    // Set position vector [m].
    Eigen::Vector3d positionVectorToSource = Eigen::Vector3d( astronomicalUnitInMeters, 0.0, 0.0 );

    // Set radiation pressure at target [N/m^2].
    const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

    // Normalize position vector to get vector pointing to source in non-dimensional units [-].
    positionVectorToSource.normalize( );

    // Set mass of accelerated body [kg].
    const double mass = 370.0;

    // Compute radiation pressure acceleration from cannonball radiation pressure model [m/s^2].
    const Eigen::Vector3d computedRadiationPressureAcceleration = electromagnetism::computeCannonBallRadiationPressureAcceleration(
                radiationPressureAtTarget, positionVectorToSource,
                areaSubjectToRadiationPressure, radiationPressureCoefficient, mass );

    // Define solar sail model settings.
    const double frontEmissivityCoefficient = 0.4;
    const double backEmissivityCoefficient = frontEmissivityCoefficient;
    const double frontLambertianCoefficient = 0.4;
    const double backLambertianCoefficient = 0.4;
    const double reflectivityCoefficient = 0.327;
    const double specularReflectionCoefficient = 1.0;
    const double coneAngle = 0.0;
    const double clockAngle = 0.0;

    // Compute velocity vector of the spacecraft w.r.t. the central body [m/s].
    Eigen::Vector3d velocityVector = std::sqrt( earthGravitationalParameter / astronomicalUnitInMeters)
        * Eigen::Vector3d( std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 );

    // Compute solar sail acceleration from solar sail model [m/s^2].
    const Eigen::Vector3d computedSolarSailAcceleration = electromagnetism::computeSolarSailAcceleration(
            frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient,
            backLambertianCoefficient, reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource,
            velocityVector.normalized(), radiationPressureAtTarget, areaSubjectToRadiationPressure, coneAngle, clockAngle, mass);

    // Check that there is no acceleration component along the x-axis.
    BOOST_CHECK_SMALL( std::fabs( computedRadiationPressureAcceleration.x( )
                                  - computedSolarSailAcceleration.x( ) ), 1.0e-8 );

    // Compare computed and expected radiation pressure acceleration vectors.
    BOOST_CHECK_SMALL( computedSolarSailAcceleration.y( ), std::numeric_limits< double >::min( ) );
    BOOST_CHECK_SMALL( computedSolarSailAcceleration.z( ), std::numeric_limits< double >::min( ) );
}

//! Test class implementation of radiation pressure acceleration model.

// Set position of source of radiation pressure at origin [m].
static Eigen::Vector3d sourcePosition = Eigen::Vector3d::Zero( );

// Get position of source of radiation pressure [m].
Eigen::Vector3d getSourcePosition( ) { return sourcePosition; }

// Set position of accelerated body [m].
static Eigen::Vector3d acceleratedBodyPosition = Eigen::Vector3d( - astronomicalUnitInMeters, 0.0, 0.0 );

// Get position of accelerated body [m].
Eigen::Vector3d getAcceleratedBodyPosition( ) { return acceleratedBodyPosition; }

// Set velocity of accelerated body [m/s].
static Eigen::Vector3d acceleratedBodyVelocity
    = std::sqrt( earthGravitationalParameter / astronomicalUnitInMeters ) * Eigen::Vector3d( 1.0 / std::sqrt( 2.0 ), 1.0 / std::sqrt( 2.0 ), 0.0 );

// Get velocity of accelerated body [m/s].
Eigen::Vector3d getAcceleratedBodyVelocity( ) { return acceleratedBodyVelocity; }

// Set velocity of central body [m/s].
static Eigen::Vector3d centralBodyVelocity
    = Eigen::Vector3d( 0.0, 0.0, 0.0 );

// Get velocity of central body [m/s].
Eigen::Vector3d getCentralBodyVelocity( ) { return centralBodyVelocity; }

// Get vector from accelerated body to source [m].
Eigen::Vector3d getVectorToSource( )
{
    return getSourcePosition( ) - getAcceleratedBodyPosition( );
}

// Set radiation pressure at location of acceleration body [N/m^2].
static double radiationPressure = radiationPressureAtOneAU
    * astronomicalUnitInMeters * astronomicalUnitInMeters / getVectorToSource( ).squaredNorm( );

// Get radiation pressure at location of acceleration body [N/m^2].
double getRadiationPressure( ) { return radiationPressure; }

// Set cone angle of the solar sail [rad].
static double coneAngle = 0.0;

// Get cone angle of the solar sail [rad].
double getConeAngle( ) { return coneAngle; }

// Set clock angle of the solar sail [rad].
static double clockAngle = 0.0;

// Get clock angle of the solar sail [rad].
double getClockAngle( ) { return clockAngle; }

// Set front emissivity coefficient of the solar sail [-].
static double frontEmissivityCoefficient = 0.4;

// Get front emissivity coefficient of the solar sail [-].
double getFrontEmissivityCoefficient( ) { return frontEmissivityCoefficient; }

// Set back emissivity coefficient of the solar sail [-].
static double backEmissivityCoefficient = 0.4;

// Get back emissivity coefficient of the solar sail [-].
double getBackEmissivityCoefficient( ) { return backEmissivityCoefficient; }

// Set front Lambertian coefficient of the solar sail [-].
static double frontLambertianCoefficient = 0.4;

// Get front Lambertian coefficient of the solar sail [-].
double getFrontLambertianCoefficient( ) { return frontLambertianCoefficient; }

// Set back Lambertian coefficient of the solar sail [-].
static double backLambertianCoefficient = 0.4;

// Get back Lambertian coefficient of the solar sail [-].
double getBackLambertianCoefficient( ) { return backLambertianCoefficient; }

// Set reflectivity coefficient of the solar sail [-].
static double reflectivityCoefficient = 0.3;

// Get reflectivity coefficient of the solar sail [-].
double getReflectivityCoefficient( ) { return reflectivityCoefficient; }

// Set specular reflection coefficient of the solar sail [-].
static double specularReflectionCoefficient = 1.0;

// Get specular reflection coefficient of the solar sail [-].
double getSpecularReflectionCoefficient( ) { return specularReflectionCoefficient; }

// Set area subject to radiation pressure [m^2].
static double areaSubjectToRadiationPressure = 2.0;

// Get area subject to radiation pressure [m^2].
double getAreaSubjectToRadiationPressure( ) { return areaSubjectToRadiationPressure; }

// Set mass of accelerated body [kg].
static double massOfAcceleratedBody = 4.0;

// Get mass of acceleration body [kg].
double getMassOfAcceleratedBody( ) { return massOfAcceleratedBody; }

// Set radiation pressure coefficient [-].
static double radiationPressureCoefficient = 1.0 + 0.3;

// Get radiation pressure coefficient [-].
double getRadiationPressureCoefficient( ) { return radiationPressureCoefficient; }

//! Test solar sail acceleration model constructor, with a solar sail model equivalent to the
//! cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassConstructor )
{

    // Declare and initialize cannon-ball radiation pressure acceleration model.
    electromagnetism::CannonBallRadiationPressurePointer radiationPressureModel
        = std::make_shared< electromagnetism::CannonBallRadiationPressureAcceleration >(
            &getSourcePosition, &getAcceleratedBodyPosition, &getRadiationPressure,
            &getRadiationPressureCoefficient, &getAreaSubjectToRadiationPressure,
            &getMassOfAcceleratedBody );
    radiationPressureModel->updateMembers( 0.0 );

    // Declare and initialize solar sail radiation acceleration model.
    electromagnetism::SolarSailAccelerationPointer solarSailModel
        = std::make_shared< electromagnetism::SolarSailAcceleration >(
            &getSourcePosition, &getAcceleratedBodyPosition, &getAcceleratedBodyVelocity,
            &getCentralBodyVelocity, &getRadiationPressure, &getConeAngle, &getClockAngle,
            &getFrontEmissivityCoefficient, &getBackEmissivityCoefficient,
            &getFrontLambertianCoefficient, &getBackLambertianCoefficient,
            &getReflectivityCoefficient, &getSpecularReflectionCoefficient,
            &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody );
    solarSailModel->updateMembers( 0.0 );

    // Compute solar sail acceleration [m/s^2].
    const Eigen::Vector3d computedSolarSailAcceleration = solarSailModel->getAcceleration( );

    // Compute radiation pressure acceleration [m/s^2].
    const Eigen::Vector3d computedRadiationPressureAcceleration = radiationPressureModel->getAcceleration( );

    // Compare computed and expected radiation pressure acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedSolarSailAcceleration,
                                       computedRadiationPressureAcceleration,
                                       1.0e-15 );
}

//! Test radiation pressure acceleration model update-members function, with a solar sail model equivalent to the
//! cannonball radiation pressure model.
BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassUpdateMembers )
{
    // Declare and initialize cannon-ball radiation pressure acceleration model.
    electromagnetism::CannonBallRadiationPressurePointer radiationPressureModel
        = std::make_shared< electromagnetism::CannonBallRadiationPressureAcceleration >(
            &getSourcePosition, &getAcceleratedBodyPosition, &getRadiationPressure,
            &getRadiationPressureCoefficient, &getAreaSubjectToRadiationPressure,
            &getMassOfAcceleratedBody );
    radiationPressureModel->updateMembers( 0.0 );

    // Declare and initialize solar sail radiation acceleration model.
    electromagnetism::SolarSailAccelerationPointer solarSailModel
        = std::make_shared< electromagnetism::SolarSailAcceleration >(
            &getSourcePosition, &getAcceleratedBodyPosition, &getAcceleratedBodyVelocity,
            &getCentralBodyVelocity, &getRadiationPressure, &getConeAngle, &getClockAngle,
            &getFrontEmissivityCoefficient, &getBackEmissivityCoefficient,
            &getFrontLambertianCoefficient, &getBackLambertianCoefficient,
            &getReflectivityCoefficient, &getSpecularReflectionCoefficient,
            &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody );
    solarSailModel->updateMembers( 0.0 );

    // Set the distance from the Sun to Venus [m].
    const double distanceSunToVenus = 0.732 * astronomicalUnitInMeters;

    // Update position of accelerated body [m].
    acceleratedBodyPosition = Eigen::Vector3d( - distanceSunToVenus / std::sqrt( 2.0 ), - distanceSunToVenus / std::sqrt( 2.0 ), 0.0 );
    acceleratedBodyVelocity = std::sqrt( earthGravitationalParameter / distanceSunToVenus )
            * Eigen::Vector3d( 1.0 / std::sqrt( 2.0 ), 1.0 / std::sqrt( 2.0 ), 0.0 );

    // Update radiation pressure at location of accelerated body [N/m^2].
    radiationPressure = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
        / getVectorToSource( ).squaredNorm( );

    // Update radiation pressure coefficient [-].
    radiationPressureCoefficient = 1.0 + 0.5;

    //Update reflectivity coefficient [-].
    reflectivityCoefficient = 0.5;

    // Update area subject to radiation pressure [m^2].
    areaSubjectToRadiationPressure = 0.005;

    // Update mass of accelerated body [kg].
    massOfAcceleratedBody = 0.0022;

    // Update class members.
    radiationPressureModel->updateMembers( 0.0 );
    solarSailModel->updateMembers( 0.0 );

    // Compute radiation pressure acceleration [m/s^2].
    const Eigen::Vector3d computedRadiationPressureAcceleration = radiationPressureModel->getAcceleration( );
    const Eigen::Vector3d computedSolarSailAcceleration = solarSailModel->getAcceleration( );

    // Check that there is no acceleration component along z-axis.
    BOOST_CHECK_SMALL( computedSolarSailAcceleration.z( ), std::numeric_limits< double >::min( ) );

    // Compare computed and expected radiation pressure acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRadiationPressureAcceleration.segment( 0, 2 ),
                                       computedSolarSailAcceleration.segment( 0, 2 ),
                                       1.0e-14 );
}

//! Test implementation of solar sail force model for a set of different cone and clock angles.
BOOST_AUTO_TEST_CASE( testImplementedSolarSailModelVsTheoreticalFirst )
{
    // Theoretical non-ideal solar sail force vectors for various test cases [N].
    std::vector< Eigen::Vector3d > expectedSolarSailForceVector;
    for ( int testCase = 0 ; testCase < 8 ; testCase++ )
    {
        expectedSolarSailForceVector.push_back(
                    ( Eigen::Vector3d( ) << - 8.52932077149712e-24, - 8.52932077149712e-24, 0.0 ).finished() );
    }
    for ( int testCase = 8 ; testCase < 12 ; testCase++ )
    {
        expectedSolarSailForceVector.push_back(
                    ( Eigen::Vector3d( ) << - 1.46272332079017e-06, - 1.46272332079017e-06, 0.0 ).finished() );
    }
    for ( int testCase = 12 ; testCase < 16 ; testCase++ )
    {
        expectedSolarSailForceVector.push_back(
                    ( Eigen::Vector3d( ) << - 5.64907119362171e-07, - 5.64907119362171e-07, 0.0 ).finished() );
    }
    for ( int testCase = 16 ; testCase < 18 ; testCase++ )
    {
        expectedSolarSailForceVector.push_back(
                    ( Eigen::Vector3d( ) << - 3.0864061085007e-08, - 3.0864061085007e-08, 0.0 ).finished() );
    }
    expectedSolarSailForceVector.push_back(
                ( Eigen::Vector3d( ) << - 1.32692625650149e-06, - 1.32692625650149e-06, 0.0 ).finished() );
    expectedSolarSailForceVector.push_back(
                ( Eigen::Vector3d( ) << - 1.80571791929449e-07, - 1.80571791929449e-07, - 1.80571791929449e-07 ).finished() );
    expectedSolarSailForceVector.push_back(
                ( Eigen::Vector3d( ) << - 3.40314752162177e-06, - 4.53753002882903e-06, - 6.80629504324355e-06 ).finished() );
    expectedSolarSailForceVector.push_back(
                ( Eigen::Vector3d( ) << - 4.73337860380423e-06, - 6.29769698345645e-06, - 7.16893475205534e-06 ).finished() );

    // Define set of cone angles for various test cases [rad].
    std::vector< double > coneAngleVector;
    coneAngleVector.push_back( mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( - mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( - mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( - mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( - mathematical_constants::PI / 2.0 );
    coneAngleVector.push_back( 0.0 );
    coneAngleVector.push_back( 0.0 );
    coneAngleVector.push_back( 0.0 );
    coneAngleVector.push_back( 0.0 );
    coneAngleVector.push_back( mathematical_constants::PI / 4.0 );
    coneAngleVector.push_back( - mathematical_constants::PI / 4.0 );
    coneAngleVector.push_back( mathematical_constants::PI / 4.0 );
    coneAngleVector.push_back( - mathematical_constants::PI / 4.0 );
    coneAngleVector.push_back( unit_conversions::convertDegreesToRadians( 80.0 ) );
    coneAngleVector.push_back( unit_conversions::convertDegreesToRadians( - 80.0 ) );
    coneAngleVector.push_back( unit_conversions::convertDegreesToRadians( 15.0 ) );
    coneAngleVector.push_back( unit_conversions::convertDegreesToRadians( 15.0 ) );
    coneAngleVector.push_back( unit_conversions::convertDegreesToRadians( 15.0 ) );
    coneAngleVector.push_back( unit_conversions::convertDegreesToRadians( 15.0 ) );

    // Define set of clock angles for various test cases [rad].
    std::vector< double  > clockAngleVector;
    clockAngleVector.push_back( 0.0 );
    clockAngleVector.push_back( mathematical_constants::PI / 2.0 );
    clockAngleVector.push_back( mathematical_constants::PI );
    clockAngleVector.push_back( 2.0 * mathematical_constants::PI );
    clockAngleVector.push_back( 0.0 );
    clockAngleVector.push_back( mathematical_constants::PI / 2.0 );
    clockAngleVector.push_back( mathematical_constants::PI );
    clockAngleVector.push_back( 2.0 * mathematical_constants::PI );
    clockAngleVector.push_back( 0.0 );
    clockAngleVector.push_back( mathematical_constants::PI / 2.0 );
    clockAngleVector.push_back( mathematical_constants::PI );
    clockAngleVector.push_back( 2.0 * mathematical_constants::PI );
    clockAngleVector.push_back( mathematical_constants::PI / 4.0 );
    clockAngleVector.push_back( - mathematical_constants::PI / 4.0 );
    clockAngleVector.push_back( - mathematical_constants::PI / 4.0 );
    clockAngleVector.push_back( mathematical_constants::PI / 4.0 );
    clockAngleVector.push_back( unit_conversions::convertDegreesToRadians( 80.0 ) );
    clockAngleVector.push_back( unit_conversions::convertDegreesToRadians( 80.0 ) );
    clockAngleVector.push_back( unit_conversions::convertDegreesToRadians( 25.0 ) );
    clockAngleVector.push_back( unit_conversions::convertDegreesToRadians( 25.0 )  );
    clockAngleVector.push_back( unit_conversions::convertDegreesToRadians( 25.0 ) );
    clockAngleVector.push_back( unit_conversions::convertDegreesToRadians( 25.0 ) );

    // Define position vector.
    Eigen::Vector3d positionVectorToSource;

    // Define velocity vector of the spacecraft w.r.t. central body.
    Eigen::Vector3d velocityVector;

    for ( int testCase = 0 ; testCase < 20 ; testCase++ ){

        // Define different position and velocity vectors for various test cases.
        if ( testCase < 19 )
        {
            // Set position vector [m].
            positionVectorToSource = Eigen::Vector3d( astronomicalUnitInMeters, astronomicalUnitInMeters, 0.0 );

            // Compute distance between spacecraft and radiation pressure source [m].
            double positionVectorToSourceNorm = positionVectorToSource.norm();

            // Set velocity vector of the spacecraft w.r.t. central body [m/s].
            velocityVector = std::sqrt( earthGravitationalParameter / positionVectorToSourceNorm )
                                       * ( Eigen::Vector3d() << std::sqrt( 2.0 ) / 2.0, std::sqrt( 2.0 ) / 2.0, 0.0 ).finished();
        }
        else if ( testCase == 19 )
        {
            // Set position vector [m].
            positionVectorToSource = 2.0 * Eigen::Vector3d( astronomicalUnitInMeters, astronomicalUnitInMeters, astronomicalUnitInMeters );

            // Compute distance between spacecraft and radiation pressure source [m].
            double positionVectorToSourceNorm = positionVectorToSource.norm();

            // Set velocity vector of the spacecraft w.r.t. central body [m/s].
            velocityVector = std::sqrt( earthGravitationalParameter / positionVectorToSourceNorm )
                                       * ( Eigen::Vector3d() << 1.0, 1.0, 1.0 ).finished();
        }
        else if ( testCase == 20 )
        {
            // Set position vector [m].
            positionVectorToSource = Eigen::Vector3d( astronomicalUnitInMeters / 4.0, astronomicalUnitInMeters / 3.0,
                                                      astronomicalUnitInMeters / 2.0 );

            // Compute distance between spacecraft and radiation pressure source [m].
            double positionVectorToSourceNorm = positionVectorToSource.norm();

            // Set velocity vector of the spacecraft w.r.t. central body [m/s].
            velocityVector = std::sqrt( earthGravitationalParameter / positionVectorToSourceNorm )
                                       * ( Eigen::Vector3d() << 1.0 / 4.0, 1.0/ 3.0, 1.0 / 2.0 ).finished();
        }
        else if ( testCase == 21 ){

            // Set position vector [m].
            positionVectorToSource = Eigen::Vector3d( astronomicalUnitInMeters / 3.5, astronomicalUnitInMeters / 3.0,
                                                      astronomicalUnitInMeters / 2.5 );

            // Compute distance between spacecraft and radiation pressure source [m].
            double positionVectorToSourceNorm = positionVectorToSource.norm();

            // Set velocity vector of the spacecraft w.r.t. central body [m/s].
            velocityVector = std::sqrt( earthGravitationalParameter / positionVectorToSourceNorm )
                                       * ( Eigen::Vector3d() << 1.0 / 2.5, 1.0 / 2.6, 1.0 / 2.9 ).finished();
        }

        // Set radiation pressure at target [N/m^2].
        const double radiationPressureAtTarget = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters
            / positionVectorToSource.squaredNorm( );

        // Normalize position vector to get vector pointing to source in non-dimensional units [-].
        positionVectorToSource.normalize( );

        // Set area on target that is subject to radiation pressure [m^2].
        const double areaSubjectToRadiationPressure = 0.5;

        // Define solar sail model settings.
        const double frontEmissivityCoefficient = 0.05;
        const double backEmissivityCoefficient = 0.64;
        const double frontLambertianCoefficient = 0.79;
        const double backLambertianCoefficient = 0.55;
        const double reflectivityCoefficient = 0.88;
        const double specularReflectionCoefficient = 0.94;

        // Compute force from solar sail model [N].
        Eigen::Vector3d computedSolarSailForce = electromagnetism::computeSolarSailForce(
                    frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient,
                    backLambertianCoefficient, reflectivityCoefficient, specularReflectionCoefficient, positionVectorToSource,
                    velocityVector.normalized(), radiationPressureAtTarget, areaSubjectToRadiationPressure,
                    coneAngleVector[ testCase ], clockAngleVector[ testCase ] );

        // Compare computed and expected radiation pressure force vectors.
        for( unsigned int j = 0; j < 3; j++ )
        {
            BOOST_CHECK_SMALL( std::fabs( expectedSolarSailForceVector[ testCase ]( j ) - computedSolarSailForce( j ) ), 1.0E-20 );
        }

    }

}

//! Test radiation pressure acceleration model update-members function, for changes in the coefficients also.
BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassUpdateMembersIncludingCoefficients )
{

    // Declare and initialize solar sail radiation acceleration model.
    electromagnetism::SolarSailAccelerationPointer solarSailModel
        = std::make_shared< electromagnetism::SolarSailAcceleration >(
            &getSourcePosition, &getAcceleratedBodyPosition, &getAcceleratedBodyVelocity,
            &getCentralBodyVelocity, &getRadiationPressure, &getConeAngle, &getClockAngle,
            &getFrontEmissivityCoefficient, &getBackEmissivityCoefficient,
            &getFrontLambertianCoefficient, &getBackLambertianCoefficient,
            &getReflectivityCoefficient, &getSpecularReflectionCoefficient,
            &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody );
    solarSailModel->updateMembers( 0.0 );

    // Set expected radiation pressure acceleration [m/s^2].
    const Eigen::Vector3d expectedSolarSailAcceleration = Eigen::Vector3d( 8.59654650354177e-06, 7.44856903789465e-06, 6.83437564207585e-06 );

    // Set the distance from the Sun to Venus [m].
    const double distanceSunToVenus = 0.732 * astronomicalUnitInMeters;

    // Update position of accelerated body [m].
    acceleratedBodyPosition = Eigen::Vector3d( distanceSunToVenus * 0.9, distanceSunToVenus * 0.8, distanceSunToVenus * 0.7 );
    const double acceleratedBodyPositionNorm = acceleratedBodyPosition.norm();
    acceleratedBodyVelocity = std::sqrt( earthGravitationalParameter / acceleratedBodyPositionNorm )
            * Eigen::Vector3d( 1.0 / std::sqrt( 2.0 ), 1.0 / std::sqrt( 2.0 ), 1.0 / std::sqrt( 2.0 ) );

    // Update radiation pressure at location of accelerated body [N/m^2].
    radiationPressure = radiationPressureAtOneAU * astronomicalUnitInMeters * astronomicalUnitInMeters / getVectorToSource( ).squaredNorm( );

    //Update cone angle [rad].
    coneAngle = 15.0 * mathematical_constants::PI / 180.0;

    //Update clock angle [rad].
    clockAngle = 25.0 * mathematical_constants::PI / 180.0;

    //Update front emissivity coefficient [-].
    frontEmissivityCoefficient=0.03;

    //Update back emissivity coefficient [-].
    backEmissivityCoefficient = 0.5;

    //Update front Lambertian coefficient [-].
    frontLambertianCoefficient = 0.70;

    //Update front Lambertian coefficient [-].
    backLambertianCoefficient = 0.4;

    //Update specular reflection coefficient [-].
    specularReflectionCoefficient = 0.9;

    //Update reflectivity coefficient [-].
    reflectivityCoefficient = 0.6;

    // Update area subject to radiation pressure [m^2].
    areaSubjectToRadiationPressure = 0.005;

    // Update mass of accelerated body [kg].
    massOfAcceleratedBody = 0.0022;

    // Update class members.
    solarSailModel->updateMembers( 0.0 );

    // Compute radiation pressure acceleration [m/s^2].
    const Eigen::Vector3d computedSolarSailAcceleration = solarSailModel->getAcceleration( );

    // Compare computed and expected radiation pressure acceleration vectors.
    BOOST_CHECK_SMALL( std::fabs( expectedSolarSailAcceleration.x() - computedSolarSailAcceleration.x()), 1.0e-19);
    BOOST_CHECK_SMALL( std::fabs( expectedSolarSailAcceleration.y() - computedSolarSailAcceleration.y()), 1.0e-19 );
    BOOST_CHECK_SMALL( std::fabs( expectedSolarSailAcceleration.z() - computedSolarSailAcceleration.z()), 1.0e-19 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


///////////////////////////////////////////////////////////////////////////////
// p_square_cumulative_distribution.hpp
//
//  Copyright 2005 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_P_SQUARE_CUMUL_DIST_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_P_SQUARE_CUMUL_DIST_HPP_DE_01_01_2006

#include <vector>
#include <functional>
#include <boost/parameter/keyword.hpp>
#include <boost/range.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/accumulators_fwd.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>

namespace boost { namespace accumulators
{
///////////////////////////////////////////////////////////////////////////////
// num_cells named parameter
//
BOOST_PARAMETER_NESTED_KEYWORD(tag, p_square_cumulative_distribution_num_cells, num_cells)

BOOST_ACCUMULATORS_IGNORE_GLOBAL(p_square_cumulative_distribution_num_cells)

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // p_square_cumulative_distribution_impl
    //  cumulative_distribution calculation (as histogram)
    /**
        @brief Histogram calculation of the cumulative distribution with the \f$P^2\f$ algorithm

        A histogram of the sample cumulative distribution is computed dynamically without storing samples
        based on the \f$ P^2 \f$ algorithm. The returned histogram has a specifiable amount (num_cells)
        equiprobable (and not equal-sized) cells.

        For further details, see

        R. Jain and I. Chlamtac, The P^2 algorithm for dynamic calculation of quantiles and
        histograms without storing observations, Communications of the ACM,
        Volume 28 (October), Number 10, 1985, p. 1076-1085.

        @param p_square_cumulative_distribution_num_cells.
    */
    template<typename Sample>
    struct p_square_cumulative_distribution_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<float_type> array_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        // for boost::result_of
        typedef iterator_range<typename histogram_type::iterator> result_type;

        template<typename Args>
        p_square_cumulative_distribution_impl(Args const &args)
          : num_cells(args[p_square_cumulative_distribution_num_cells])
          , heights(num_cells + 1)
          , actual_positions(num_cells + 1)
          , desired_positions(num_cells + 1)
          , positions_increments(num_cells + 1)
          , histogram(num_cells + 1)
          , is_dirty(true)
        {
            std::size_t b = this->num_cells;

            for (std::size_t i = 0; i < b + 1; ++i)
            {
                this->actual_positions[i] = i + 1.;
                this->desired_positions[i] = i + 1.;
                this->positions_increments[i] = numeric::average(i, b);
            }
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            this->is_dirty = true;

            std::size_t cnt = count(args);
            std::size_t sample_cell = 1; // k
            std::size_t b = this->num_cells;

            // accumulate num_cells + 1 first samples
            if (cnt <= b + 1)
            {
                this->heights[cnt - 1] = args[sample];

                // complete the initialization of heights by sorting
                if (cnt == b + 1)
                {
                    std::sort(this->heights.begin(), this->heights.end());
                }
            }
            else
            {
                // find cell k such that heights[k-1] <= args[sample] < heights[k] and adjust extreme values
                if (args[sample] < this->heights[0])
                {
                    this->heights[0] = args[sample];
                    sample_cell = 1;
                }
                else if (this->heights[b] <= args[sample])
                {
                    this->heights[b] = args[sample];
                    sample_cell = b;
                }
                else
                {
                    typename array_type::iterator it;
                    it = std::upper_bound(
                        this->heights.begin()
                      , this->heights.end()
                      , args[sample]
                    );

                    sample_cell = std::distance(this->heights.begin(), it);
                }

                // increment positions of markers above sample_cell
                for (std::size_t i = sample_cell; i < b + 1; ++i)
                {
                    ++this->actual_positions[i];
                }

                // update desired position of markers 2 to num_cells + 1
                // (desired position of first marker is always 1)
                for (std::size_t i = 1; i < b + 1; ++i)
                {
                    this->desired_positions[i] += this->positions_increments[i];
                }

                // adjust heights of markers 2 to num_cells if necessary
                for (std::size_t i = 1; i < b; ++i)
                {
                    // offset to desire position
                    float_type d = this->desired_positions[i] - this->actual_positions[i];

                    // offset to next position
                    float_type dp = this->actual_positions[i + 1] - this->actual_positions[i];

                    // offset to previous position
                    float_type dm = this->actual_positions[i - 1] - this->actual_positions[i];

                    // height ds
                    float_type hp = (this->heights[i + 1] - this->heights[i]) / dp;
                    float_type hm = (this->heights[i - 1] - this->heights[i]) / dm;

                    if ( ( d >= 1. && dp > 1. ) || ( d <= -1. && dm < -1. ) )
                    {
                        short sign_d = static_cast<short>(d / std::abs(d));

                        // try adjusting heights[i] using p-squared formula
                        float_type h = this->heights[i] + sign_d / (dp - dm) * ( (sign_d - dm) * hp + (dp - sign_d) * hm );

                        if ( this->heights[i - 1] < h && h < this->heights[i + 1] )
                        {
                            this->heights[i] = h;
                        }
                        else
                        {
                            // use linear formula
                            if (d>0)
                            {
                                this->heights[i] += hp;
                            }
                            if (d<0)
                            {
                                this->heights[i] -= hm;
                            }
                        }
                        this->actual_positions[i] += sign_d;
                    }
                }
            }
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                // creates a vector of std::pair where each pair i holds
                // the values heights[i] (x-axis of histogram) and
                // actual_positions[i] / cnt (y-axis of histogram)

                std::size_t cnt = count(args);

                for (std::size_t i = 0; i < this->histogram.size(); ++i)
                {
                    this->histogram[i] = std::make_pair(this->heights[i], numeric::average(this->actual_positions[i], cnt));
                }
            }
            //return histogram;
            return make_iterator_range(this->histogram);
        }

    private:
        std::size_t num_cells;            // number of cells b
        array_type  heights;              // q_i
        array_type  actual_positions;     // n_i
        array_type  desired_positions;    // n'_i
        array_type  positions_increments; // dn'_i
        mutable histogram_type histogram; // histogram
        mutable bool is_dirty;
    };

} // namespace detail

///////////////////////////////////////////////////////////////////////////////
// tag::p_square_cumulative_distribution
//
namespace tag
{
    struct p_square_cumulative_distribution
      : depends_on<count>
      , p_square_cumulative_distribution_num_cells
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::p_square_cumulative_distribution_impl<mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::p_square_cumulative_distribution
//
namespace extract
{
    extractor<tag::p_square_cumulative_distribution> const p_square_cumulative_distribution = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(p_square_cumulative_distribution)
}

using extract::p_square_cumulative_distribution;

// So that p_square_cumulative_distribution can be automatically substituted with
// weighted_p_square_cumulative_distribution when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::p_square_cumulative_distribution>
{
    typedef tag::weighted_p_square_cumulative_distribution type;
};

template<>
struct feature_of<tag::weighted_p_square_cumulative_distribution>
  : feature_of<tag::p_square_cumulative_distribution>
{
};

}} // namespace boost::accumulators

#endif


#ifndef NDSS_VECTOR_HPP
#define NDSS_VECTOR_HPP
#include <vector>
#include <iostream>
#include <NTL/ZZX.h>
class EncryptedArray;
namespace MDL {
template<typename T>
class Vector : public std::vector<T> {
public:
    Vector(long dims = 0) : std::vector<T>(dims) {}

    Vector(long dims, const T &init) : std::vector<T>(dims, init) {}

    Vector& operator=(const std::vector<T> &oth) {
        auto tmp(oth);
        this->swap(tmp);
        return *this;
    }

    T dot(const Vector<T> &oth) const;

    size_t dimension() const {
        return this->size();
    }

    double L2() const;

    template<typename U>
    friend std::ostream& operator<<(std::ostream& os,
                                    Vector<U>   & obj);

    Vector<double> reduce(double factor) const;

    NTL::ZZX encode(const EncryptedArray &ea) const;

    void random(const T &domain);

    Vector& operator*=(const T &val);

    Vector& operator+=(const Vector<T> &oth);

    Vector& operator-=(const Vector<T> &oth);

    Vector<T> subvector(long startIndex, long endIndex) const;

    Vector<long> div(long factor) const;
};

template<typename U>
std::ostream& operator<<(std::ostream& os, Vector<U>& obj)
{
    std::cout << "[";

    for (auto& e : obj) {
        std::cout << " " << e;
    }
    std::cout << " ]";
    return os;
}
} // namespace MDL
#endif // ifndef NDSS_VECTOR_HPP


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_TIME_CONVERSIONS
#define BOOST_ASTRONOMY_TIME_CONVERSIONS

#include <string>
#include <exception>
#include <boost/astronomy/time/parser.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace boost { namespace astronomy { namespace time {

/**
 * Universal time (UT), and therefore the local civil time in any
 * part of the world, is related to the apparent motion of the Sun
 * around the Earth.
 *
 * Sidereal Time (ST) is a time scale that is based on Earth's rate
 * of rotation measured relative to the fixed stars.
 */

/**
 * The Greenwich Sidereal Time(GST) is the sidereal time correct for observations
 * made on the Greenwich meridian, longitude 0◦.
 */

double julian_date(boost::posix_time::ptime t)
{
    //Get date from UT
    boost::gregorian::date dt = t.date();

    //Set y = year, m = month and d = day
    double y = dt.year();
    double m = dt.month();
    double d = dt.day();

    //If m = 1 or 2, set yprime = y − 1 and mprime = m + 12; otherwise yprime = y and mprime = m.
    double yprime;
    double mprime;
    if ( (m == 1) || (m == 2) )
    {
        yprime = y - 1;
        mprime = m + 12;
    }
    else
    {
        yprime = y;
        mprime = m;
    }

    //Calculate B, check if the date is later than 1582 October 15
    double B;
    boost::gregorian::date dt1(1582, boost::gregorian::Oct , 1);
    if ( dt > dt1 )
    {
        double A = floor(yprime / 100);
        B = 2 - A + floor(A / 4);
    }
    else
    {
        B = 0;
    }

    //Calculate C, check if yprime is negative
    double C;
    if (yprime < 0)
    {
        C = floor((365.25 * yprime) - 0.75);
    }
    else
    {
        C = floor(365.25 * yprime);
    }

    //Calculate D
    double D;
    D = floor(30.6001 * (mprime + 1));

    //Finding Julian date
    double JD;
    JD = B + C + D + d + 1720994.5;

    return JD;
}

decimal_hour GST(boost::posix_time::ptime t)
{
    //Get Julian Day Number
    double JD = julian_date(t);

    double S = JD - 2451545.0;

    double T = S/36525.0;

    double T0 = 6.697374558 + (2400.051336 * T) + (0.000025862 * T * T);

    //Reduce the result to the range 0 to 24 by adding or subtracting multiples of 24
    T0 = T0 - 24.0 * floor(T0/24.0);

    //Convert UT to decimal hours
    double UT = ((t.time_of_day().seconds())/60.0 + t.time_of_day().minutes())/60.0 + t.time_of_day().hours();

    //Multiply UT by 1.002737909
    double A = UT * 1.002737909;

    T0 += A;

    //Add this to T0 and reduce to the range 0 to 24 if necessary by subtracting or adding 24. This is the GST.
    T0 = T0 - 24.0 * floor(T0/24.0);

    //Return GST in decimal hours
    return {T0};
}

enum class DIRECTION {WEST, EAST};

//Local Sidereal Time (LST)
decimal_hour LST(double longitude, DIRECTION direction, boost::posix_time::ptime t)
{
    double gst = GST(t).get();

    if(longitude == 0)
      return {gst};

    //Convert longitude to hours
    double long_hours = longitude / 15.0;

    switch(direction)
    {
      case DIRECTION::WEST:
          //Multiply with direction
          long_hours = -1 * long_hours;
            break;
      case DIRECTION::EAST:
          //Multiply with direction
          long_hours = 1 * long_hours;
            break;
    }

    long_hours = long_hours + gst;

    //Bring the result into the range 0 to 24 by adding or subtracting 24 if necessary.
    //This is the local sidereal time (LST).
    long_hours = long_hours - 24.0 * floor(long_hours/24.0);

    return {long_hours};
}

}}} // namespace::astronomy::time
#endif //BOOST_ASTRONOMY_TIME_CONVERSIONS


#include "element.h"
#include <armadillo>

//#define NDEBUG 1
#include <cassert>

using namespace arma;

// TODO let's store material properties in a struct or an instance and only keep a reference to it
// TODO most matrices we can make a fixed, static size. implement this for better performance
// TODO consider a more flyweight approach... how much can we slim down cost of creating an element?
// TODO convert c style arrays to c++ arrays
// TODO separate material data from mesh data
// TODO convert for loops to ranged loops
// TODO delegate integration to a separate "Integration" class

/**
 * Default constructor for a mechanical finite element
 *
 * @return MechElem
 */
MechElem::MechElem() {}

/**
 * Constructor for a mechanical finite element
 *
 * @param [int] E Modulus of elasticity
 * @param [double] v Poissons ratio
 * @param [vec*] b Body forces: x-direction, y-direction
 * @param [vec*] t Traction forces: bottom, right, top, left
 * @param [int*] gnodes Nodal numbers: bot-left, bot-right, top-right, top-left
 * @param [mat*] gcoords Global coordinates corresponding to each node
 * @param [int*] gdof Global degrees of freedom
 * @return MechElem
 */
MechElem::MechElem(long long int E, double v, vec *pb, vec *pt, int *gnodes,
        mat *pgcoords, int *gdofs) : E(E), v(v), pb(pb), pt(pt),
        gnodes(gnodes), pgcoords(pgcoords), gdofs(gdofs) {};

/**
 * Clone of a mechanical finite element
 *
 * @param [int]
 * @return MechElem
 */
MechElem::MechElem(const MechElem& orig)
{
}

/**
 * Deallocation of a mechanical finite element
 *
 * @param void
 * @return void
 */
MechElem::~MechElem(void)
{
}

/**
 * Accessor for global nodal coordinates
 *
 * @return mat *gcoords
 */
mat *MechElem::getGcoords() { return pgcoords; }

/**
 * Accessor for global node numbers
 *
 * @return int *gnodes
 */
int *MechElem::getGnodes() { return gnodes; }

/**
 * Accessor for global degrees of freedom
 *
 * @return int *gdofs
 */
int *MechElem::getGdofs() { return gdofs; }

/*
 * Q4 class constants
 */
double const Q4::gaussPoints[] = { -0.5774, 0.5774 };
int const Q4::weights[] = { 1, 1 };

/**
 * Default constructor for a Q4 finite element
 *
 * @return Q4
 */
Q4::Q4() {}

// TODO: consider separating material data from element object

/**
 * Constructor for a Q4 finite element
 *
 * @param [int] E Modulus of elasticity
 * @param [double] v Poissons ratio
 * @param [double] h Thickness or 'height' of the element
 * @param [vec] b Body forces: x-direction, y-direction
 * @param [vec] t Traction forces: bottom, right, top, left
 * @param [int*] gnodes Nodal numbers: bot-left, bot-right, top-right, top-left
 * @param [mat*] gcoords Global coordinates corresponding to each node
 * @param [int*] gdof Global degrees of freedom
 * @return Q4
 */
Q4::Q4(long long int E, double v, double h, vec *pb, vec *pt, int *gnodes,
    mat *pgcoords, int *gdofs) : MechElem(E, v, pb, pt, gnodes, pgcoords,
    gdofs), h(h) {};

/**
 * Deallocation of a Q4 finite element
 *
 * @param void
 * @return void
 */
Q4::~Q4(void)
{
}

// consider inline functions?
/*
 * Define the Q4 shape functions
 */
#define Q4__N1(xi, eta) (1-xi)*(1-eta)/4
#define Q4__N2(xi, eta) (1+xi)*(1-eta)/4
#define Q4__N3(xi, eta) (1+xi)*(1+eta)/4
#define Q4__N4(xi, eta) (1-xi)*(1+eta)/4

/**
 * Calculates the N matrix
 *
 * @param [double] xi Xi coordinate in parent coordinates
 * @param [double] eta Eta coordinate in parent coordinates
 * @return [mat] N matrix
 */
mat Q4::N(double xi, double eta)
{
    mat::fixed<Q4__DOF_PER_NODE,Q4__DOF_PER_NODE*Q4__NUM_NODES> N;
    N << Q4__N1(xi, eta) << 0 << Q4__N2(xi, eta) << 0 << Q4__N3(xi, eta) << 0
            << Q4__N4(xi, eta) << 0 << endr
      << 0 << Q4__N1(xi, eta) << 0 << Q4__N2(xi, eta) << 0 << Q4__N3(xi, eta)
            << 0 << Q4__N4(xi, eta) << endr;
    return N;
}

/*
 * Define the partials of the Q4 shape functions with respect to xi and eta
 */
#define Q4__dN1_dxi(eta) -(1-eta)/4
#define Q4__dN2_dxi(eta) (1-eta)/4
#define Q4__dN3_dxi(eta) (1+eta)/4
#define Q4__dN4_dxi(eta) -(1+eta)/4
#define Q4__dN1_deta(xi) -(1-xi)/4
#define Q4__dN2_deta(xi) -(1+xi)/4
#define Q4__dN3_deta(xi) (1+xi)/4
#define Q4__dN4_deta(xi) (1-xi)/4

/**
 * Calculates the jacobian
 *
 * @param [double] xi Xi coordinate in parent coordinates
 * @param [double] eta Eta coordinate in parent coordinates
 * @return [mat] Jacobian
 */
mat Q4::J(double xi, double eta)
{
    mat::fixed<Q4__DOF_PER_NODE,Q4__NUM_NODES> dj;
    dj << Q4__dN1_dxi(eta) << Q4__dN2_dxi(eta) << Q4__dN3_dxi(eta)
            << Q4__dN4_dxi(eta) << endr
       << Q4__dN1_deta(xi) << Q4__dN2_deta(xi) << Q4__dN3_deta(xi)
            << Q4__dN4_deta(xi) << endr;
    mat jacobian = dj * *pgcoords;
    return jacobian;
}

// TODO: have Q4 inherit from 'shell element' in order to maximize code reuse

/**
 * Calculate the element body force, mutator
 *
 * @param [vec&] bodyForce Body force vector
 * @return void
 */
void Q4::mbodyForce(vec &bodyForce)
{
    assert(bodyForce.n_rows == Q4__DOF_PER_NODE*Q4__NUM_NODES);
    // TODO make zeros at or not at the beg of function consistent across functions
    bodyForce.zeros();
    unsigned int i, j;
    double xi, eta, weightX, weightY;

    /*
     * calculate element body force using gauss quadrature
     */
    for (i = 0; i < Q4::numPointsX; i++)
    {
        xi = Q4::gaussPoints[i];
        weightX = Q4::weights[i];
        for (j = 0; j < Q4::numPointsY; j++)
        {
            eta = Q4::gaussPoints[j];
            weightY = Q4::weights[j];
            bodyForce += weightX * weightY * trans(N(xi, eta)) * *pb
                    * det(J(xi, eta));
        }
    }
    bodyForce *= h;
}

//TODO: fix this function
/**
 * Calculates the traction for a surface, mutator
 *
 * @param [vec&] traction Traction force vector
 * @return void
 */
void Q4::mtraction(vec &traction)
{
    traction.zeros();
    unsigned int i, j;
    double xi, eta, weightX, weightY;

    /*
     * calculate element body force using gauss quadrature
     */
    for (i = 0; i < Q4::numPointsX; i++)
    {
        xi = Q4::gaussPoints[i];
        weightX = Q4::weights[i];
        for (j = 0; j < Q4::numPointsY; j++)
        {
            eta = Q4::gaussPoints[j];
            weightY = Q4::gaussPoints[j];
            traction += weightX * weightY * trans(N(xi, eta)) * *pt
                    * det(J(xi, eta));
        }
    }
    traction *= h;
}

#define Q4__STRAIN_COMP 3

/**
 * Calculates the stiffness matrix for the element, mutator
 *
 * @param [mat*] stiff Element stiffness matrix
 * @param [PlaneState] pState Plane state control
 * @return void
 */
void Q4::mstiffness(mat &stiff, int pState = PSTRESS)
{
    assert(stiff.n_rows == Q4__DOF_PER_NODE*Q4__NUM_NODES);
    assert(stiff.n_cols == Q4__DOF_PER_NODE*Q4__NUM_NODES);

    stiff.zeros();
    Mat<int>::fixed<3,4> e;
    e << 1 << 0 << 0 << 0 << endr
      << 0 << 0 << 0 << 1 << endr
      << 0 << 1 << 1 << 0 << endr;
    unsigned int i, j, k, m;
    double xi, eta, weightX, weightY, temp;
    mat nStar = zeros<mat>(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,Q4__DOF_PER_NODE*
                Q4__NUM_NODES);
    mat jacobian(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,
            Q4__DOF_PER_NODE*Q4__DOF_PER_NODE);
    mat invJac(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,
            Q4__DOF_PER_NODE*Q4__DOF_PER_NODE);
    mat JE = zeros<mat>(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,
        Q4__DOF_PER_NODE*Q4__DOF_PER_NODE);
    mat B(Q4__STRAIN_COMP,Q4__DOF_PER_NODE*Q4__NUM_NODES);
    mat C(Q4__STRAIN_COMP,Q4__STRAIN_COMP);

    /* TODO: separate this logic and material properties from element so that
     *          the C matrix will not need calculated a billion times */
    // PROB AN EXPENSIVE OPERATION, DO SOMETHING ABOUT THIS
    switch (pState)
    {
        case PSTRESS:
            C << 1 << v << 0 << endr
              << v << 1 << 0 << endr
              << 0 << 0 << (1-v)/2 << endr;
            C *= E/(1-v*v);
            break;
        case PSTRAIN:
            C << 1-v << v << 0 << endr
              << v << 1-v << 0 << endr
              << 0 << 0 << (1-2*v)/2 << endr;
            C *= E/((1-v)*(1-2*v));
            break;
        default:
            // TODO: IDK, FIX THIS I GUESS
            cout << "Cannot understand plane state" << endl;
            assert(0);
    }

    /*
     * calculate element stiffness matrix using gauss quadrature
     */
    for (i = 0; i < Q4::numPointsX; i++)
    {
        xi = Q4::gaussPoints[i];
        weightX = Q4::weights[i];
        /*
         * create "nStar" matrix
         */
        for (j = 1; j < Q4__DOF_PER_NODE*Q4__DOF_PER_NODE; j+=2)
        {
            nStar(j,j/2) = Q4__dN1_deta(xi);
            nStar(j,j/2+2) = Q4__dN2_deta(xi);
            nStar(j,j/2+4) = Q4__dN3_deta(xi);
            nStar(j,j/2+6) = Q4__dN4_deta(xi);
        }
        /*
         * look up 'eta'
         * look up weight in the y-direction
         * calculate jacobian
         * build JE matrix
         * finish building nStar
         */
        for (j = 0; j < Q4::numPointsY; j++)
        {
            eta = Q4::gaussPoints[j];
            weightY = Q4::weights[j];
            jacobian = J(xi, eta);
            invJac = inv(jacobian);
            for (k = 0; k < Q4__DOF_PER_NODE; k++)
            {
                for (m = 0; m < Q4__DOF_PER_NODE; m++)
                {
                    temp = invJac(k,m);
                    JE(k,m) = temp;
                    JE(k+Q4__DOF_PER_NODE,m+Q4__DOF_PER_NODE) = temp;
                }
            }

            for (k = 0; k < Q4__DOF_PER_NODE*Q4__DOF_PER_NODE; k+=2)
            {
                nStar(k,k/2) = Q4__dN1_dxi(eta);
                nStar(k,k/2+2) = Q4__dN2_dxi(eta);
                nStar(k,k/2+4) = Q4__dN3_dxi(eta);
                nStar(k,k/2+6) = Q4__dN4_dxi(eta);
            }
            // this is derived in class notes (Aquino)
            B = e * JE * nStar;
            stiff += weightX * weightY * trans(B) * C * B * det(jacobian);
        }
    }
    stiff *= h;
}

//TODO implement Q4R element


#include <armadillo>

// on laptop:
// size_t      <- long unsigned int
// arma::uword <- long long unsigned int
typedef arma::uword index_t;

int main() {

    const size_t n_rows = 10;
    const size_t n_cols = 4;
    const size_t n_slices = 2;
    const int imin = -3;
    const int imax = 10;

    const arma::Cube<int> R = arma::randi<arma::Cube<int>>(n_rows, n_cols, n_slices, arma::distr_param(imin, imax));

    R.print("R");

    index_t _indices[5] = {2, 3, 4, 8, 9};
    const arma::uvec indices = arma::uvec(&_indices[0], 5, true);
    indices.print("indices");

    return 0;
}


#include <stdlib.h>
#include <iostream>
#include <memory>
#include "conex/cone_program.h"
#include "conex/constraint.h"
#include "conex/equality_constraint.h"
#include "conex/linear_constraint.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {
using DenseMatrix = Eigen::MatrixXd;
using Eigen::MatrixXd;
using Eigen::VectorXd;

auto GetConfiguration() {
  SolverConfiguration config;
  config.prepare_dual_variables = true;
  config.inv_sqrt_mu_max = 5e5;
  config.divergence_upper_bound = 1000;
  config.dinf_upper_bound = 1.35;
  config.final_centering_tolerance = 1;
  return config;
}

int DoRandomDenseTest(const SolverConfiguration& config, int number_of_tests,
                      int random_seed) {
  srand(random_seed);
  int total_iters = 0;
  for (int i = 0; i < number_of_tests; i++) {
    int num_variables = 5;
    int num_constraints = 6 + 2 * i;
    double eps = 1e-12;

    DenseMatrix Alinear = DenseMatrix::Random(num_constraints, num_variables);
    DenseMatrix Clinear = DenseMatrix::Random(num_constraints, 1);
    Clinear = Clinear.array().abs();

    LinearConstraint linear_constraint{num_constraints, &Alinear, &Clinear};

    Program prog(num_variables);
    prog.AddConstraint(linear_constraint);

    VectorXd x0 = VectorXd::Random(num_constraints, 1);
    x0 = x0.array().abs();
    x0 *= 0.01 / x0.norm();
    VectorXd b = Alinear.transpose() * x0;
    DenseMatrix y(num_variables, 1);
    Solve(b, prog, config, y.data());

    VectorXd x(num_constraints);
    prog.GetDualVariable(0, &x);

    VectorXd slack = Clinear - Alinear * y;
    EXPECT_LE((Alinear.transpose() * x - b).norm(), eps * b.norm());
    EXPECT_GE(slack.minCoeff(), -eps);
    EXPECT_GE(x.minCoeff(), -eps);
    EXPECT_GE(slack.dot(x), -eps);
    double mu = 1.0 / (config.inv_sqrt_mu_max * config.inv_sqrt_mu_max);
    EXPECT_LE(slack.dot(x), (mu + std::sqrt(eps)) * num_constraints);
    auto status = prog.Status();
    total_iters += status.num_iterations;
  }
  return total_iters;
}

int num_tests = 3;
int random_seed = 1;

GTEST_TEST(KKTSolver, UseIterativeRefinement) {
  auto config = GetConfiguration();
  config.iterative_refinement_iterations = 3;
  config.kkt_solver = CONEX_LLT_FACTORIZATION;
  DoRandomDenseTest(config, num_tests, random_seed);
}

GTEST_TEST(KKTSolver, UseLLT) {
  auto config = GetConfiguration();
  config.iterative_refinement_iterations = 0;
  config.kkt_solver = CONEX_LLT_FACTORIZATION;
  DoRandomDenseTest(config, num_tests, random_seed);
}

GTEST_TEST(LP, UseLDLT) {
  auto config = GetConfiguration();
  config.kkt_solver = CONEX_LDLT_FACTORIZATION;
  DoRandomDenseTest(config, num_tests, random_seed);
}

GTEST_TEST(LP, UseQR) {
  auto config = GetConfiguration();
  config.kkt_solver = CONEX_QR_FACTORIZATION;
  DoRandomDenseTest(config, num_tests, random_seed);
}

GTEST_TEST(QR, SuccessWithDependentInequalityColumns) {
  auto config = GetConfiguration();
  config.kkt_solver = CONEX_QR_FACTORIZATION;
  Eigen::MatrixXd A(3, 4);
  Eigen::VectorXd c(3);
  A << 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0;
  c << 1, 1, 1;

  Program prog(4);
  prog.AddConstraint(LinearConstraint{A, c});
  MatrixXd b = A.transpose() * c;

  VectorXd solution(4);
  Solve(b, prog, config, solution.data());
  EXPECT_EQ(prog.Status().solved, 1);

  config.kkt_solver = CONEX_LLT_FACTORIZATION;
  Solve(b, prog, config, solution.data());
  EXPECT_EQ(prog.Status().solved, 0);
}

GTEST_TEST(QR, SuccessWithDependentEquations) {
  auto config = GetConfiguration();
  Eigen::MatrixXd A(3, 4);
  Eigen::VectorXd c(3);
  A << 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0;
  c << 1, 1, 1;

  MatrixXd B(5, 4);
  VectorXd d(5);
  B << 1, -1, 0, 0, 1, -1, 0, 0, 2, -2, 0, 0, 3, -3, 0, 0, 1, -1, 0, 0;
  d << 0, 0, 0, 0, 0;

  Program prog(4);
  prog.AddConstraint(LinearConstraint{A, c});
  prog.AddConstraint(EqualityConstraints{B, d});
  MatrixXd b = A.transpose() * c;

  VectorXd solution(4);
  config.kkt_solver = CONEX_QR_FACTORIZATION;
  Solve(b, prog, config, solution.data());
  EXPECT_EQ(prog.Status().solved, 1);
  EXPECT_NEAR((B * solution - d).norm(), 0, 1e-9);

  config.kkt_solver = CONEX_LDLT_FACTORIZATION;
  Solve(b, prog, config, solution.data());
  EXPECT_EQ(prog.Status().solved, 1);
  EXPECT_NEAR((B * solution - d).norm(), 0, 1e-9);
}

}  // namespace conex


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#ifndef ROKKO_UTILITY_XYZ_HAMILTONIAN_MPI_HPP
#define ROKKO_UTILITY_XYZ_HAMILTONIAN_MPI_HPP

#include "mpi.h"
#include <vector>
#include <boost/tuple/tuple.hpp>

#include <iostream>

#include <rokko/distributed_matrix.hpp>
#include <rokko/localized_matrix.hpp>

namespace rokko {

namespace xyz_hamiltonian {

void multiply(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, const double* v, double* w, double* buffer) {
  int myrank, nproc;
  MPI_Status status;
  int ierr;

  MPI_Comm_size(comm, &nproc);
  MPI_Comm_rank(comm, &myrank);

  int n = nproc;
  int p = -1;
  do {
    n /= 2;
    ++p;
  } while (n > 0);

  if (nproc != (1 << p)) {
    if ( myrank == 0 ) {
      std::cout << "This program can be run only for powers of 2" << std::endl;
    }
    MPI_Abort(comm, 1);
  }
  int N = 1 << (L-p);

  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();

    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    if (i < (L-p)) {
      if (j < (L-p)) {
        int m1 = 1 << i;
        int m2 = 1 << j;
        int m3 = m1 + m2;
        for (int k=0; k<N; ++k) {
          if (((k & m3) == m1) || ((k & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
            w[k] += diag_minus * v[k] + offdiag_plus * v[k^m3];
          } else {
            w[k] += diag_plus * v[k] + offdiag_minus * v[k^m3];
          }
        }
      } else {
        int m = 1 << (j-(L-p));
        MPI_Sendrecv(const_cast<double*>(&v[0]), N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     &buffer[0], N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     comm, &status);
        int m1 = 1 << i;
        if ((myrank & m) == m) {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            } else {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            }
          }
        } else {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            } else {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            }
          }
        }
      }
    } else {
      if (j < (L-p)) {
        int m = 1 << (i-(L-p));
        MPI_Sendrecv(const_cast<double*>(&v[0]), N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     &buffer[0], N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     comm, &status);
        int m1 = 1 << j;
        if ((myrank & m) == m) {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            } else {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            }
          }
        } else {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            } else {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            }
          }
        }
      } else {
        int m = (1 << (i-(L-p))) + (1 << (j-(L-p)));
        MPI_Sendrecv(const_cast<double*>(&v[0]), N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     &buffer[0], N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     comm, &status);
        if (((myrank & m) != m) && ((myrank & m) != 0)) {
          for (int k=0; k<N; ++k) {
            w[k] += diag_minus * v[k] + offdiag_plus * buffer[k];
          }
        } else {
          for (int k=0; k<N; ++k) {
            w[k] += diag_plus * v[k] + offdiag_minus * buffer[k];
          }
        }
      }
    }
  }
}

void multiply(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, const std::vector<double>& v, std::vector<double>& w, std::vector<double>& buffer) {
  multiply(comm, L, lattice, coupling, &v[0], &w[0], &buffer[0]);
}

void fill_diagonal(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, double* w) {
  int myrank, nproc;

  MPI_Comm_size(comm, &nproc);
  MPI_Comm_rank(comm, &myrank);

  int n = nproc;
  int p = -1;
  do {
    n /= 2;
    ++p;
  } while (n > 0);

  if (nproc != (1 << p)) {
    if ( myrank == 0 ) {
      std::cout << "This program can be run only for powers of 2" << std::endl;
    }
    MPI_Abort(comm, 1);
  }

  int N_seq = 1 << L;
  int N = 1 << (L-p);
  int myrank_shift = myrank * N;
  int nproc_shift = (nproc-1) * N;
  int mask = N - 1;

  for (int k=0; k<N; ++k) {
    w[k] = 0;
  }

  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;

    for (int k=0; k<N_seq; ++k) {
      if (myrank_shift == (k & nproc_shift)) {
        if (((k & m3) == m1) || ((k & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
          w[k & mask] += diag_minus;
        } else {
          w[k & mask] += diag_plus;
        }
      }
    }  // end for k
  } // end for lattice
}

void fill_diagonal(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, std::vector<double>& w) {
  fill_diagonal(comm, L, lattice, coupling, &w[0]);
}

template<typename T, typename MATRIX_MAJOR>
void generate(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling,
  rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
  mat.set_zeros();
  int N = 1 << L;
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz/ 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;

    for (int k=0; k<N; ++k) {
      if (mat.is_gindex_mycol(k)) {
        int local_k = mat.translate_g2l_col(k);
        if (((k & m3) == m1) || ((k & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
          if (mat.is_gindex_myrow(k^m3)) {
            mat.update_local(mat.translate_g2l_row(k^m3), local_k, offdiag_plus);
          }
          if (mat.is_gindex_myrow(k)) {
            mat.update_local(mat.translate_g2l_row(k), local_k, diag_minus);
          }
        } else {
          if (mat.is_gindex_myrow(k^m3)) {
            mat.update_local(mat.translate_g2l_row(k^m3), local_k, offdiag_minus);
          }
          if (mat.is_gindex_myrow(k)) {
            mat.update_local(mat.translate_g2l_row(k), local_k, diag_plus);
          }
        }
      }
    }
  }
}

// The following routine uses local indices.  It works correctly.
/*
template <typename MATRIX_MAJOR>
void generate(int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, rokko::distributed_matrix<MATRIX_MAJOR>& mat) {
  mat.set_zeros();
  int N = 1 << L;
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz/ 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
      int k1 = mat.translate_l2g_row(local_i);
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int k2 = mat.translate_l2g_col(local_j);
        if (((k2 & m3) == m1) || ((k2 & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
          if (k1 == (k2^m3)) {
            mat.update_local(local_i, local_j, offdiag_plus);
          }
          if (k1 == k2) {
            mat.update_local(local_i, local_j, diag_minus);
          }
        } else {
          if (k1 == (k2^m3)) {
            mat.update_local(local_i, local_j, offdiag_minus);
          }
          if (k1 == k2) {
            //std::cout << "k1=" << k1 << " k2=" << k2 << std::endl;
            mat.update_local(local_i, local_j, diag_plus);
          }
        }
      }
    }
  }
}
*/

} // namespace xyz_hamiltonian

} // namespace rokko

#endif // ROKKO_UTILITY_XYZ_HAMILTONIAN_MPI_HPP