/*ckwg +29
 * Copyright 2013-2014 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief Implementation of \link maptk::rotation_ rotation_<T> \endlink
 *        for \c T = { \c float, \c double }
 */

#include "rotation.h"

#include <cmath>
#include <limits>
#include <boost/math/constants/constants.hpp>

namespace
{

/// helper function to covert axis/angle into quaternion
template <typename T>
maptk::vector_4_<T>
quaternion_from_axis_angle(const maptk::vector_<3,T>& axis, T angle)
{
  maptk::vector_4_<T> q;
  T a = angle / T(2);
  T sa = std::sin(a);
  q[0] = sa * axis[0];
  q[1] = sa * axis[1];
  q[2] = sa * axis[2];
  q[3] = std::cos(a);
  return q;
}

} // end anonymous namespace

namespace maptk
{

/// Constructor - from a Rodrigues vector
template <typename T>
rotation_<T>
::rotation_(const vector_<3,T>& rvec)
{
  T mag = rvec.magnitude();
  if (mag == T(0))
  {
    // identity rotation is a special case
    q_ = vector_4_<T>(0,0,0,1);
  }
  else
  {
    q_ = quaternion_from_axis_angle(rvec/mag, mag);
  }
}

/// Constructor - from rotation angle and axis
template <typename T>
rotation_<T>
::rotation_(T angle, const vector_<3,T>& axis)
  : q_(quaternion_from_axis_angle(normalized(axis), angle))
{
}

/// Constructor - from yaw, pitch, and roll
template <typename T>
rotation_<T>
::rotation_(const T& yaw, const T& pitch, const T& roll)
{
  using std::sin;
  using std::cos;
  // compute the rotation from North-East-Down (NED) coordinates to
  // East-North-Up coordinates (ENU). It is a 180 degree rotation about
  // the axis [1/sqrt(2), 1/sqrt(2), 0]
  const double root_two = boost::math::constants::root_two<double>();
  const T inv_root_two = static_cast<T>(1.0/root_two);
  const rotation_<T> Rned2enu(vector_4_<T>(inv_root_two, inv_root_two, 0, 0));
  const double half_x = 0.5 * static_cast<double>(-roll);
  const double half_y = 0.5 * static_cast<double>(-pitch);
  const double half_z = 0.5 * static_cast<double>(-yaw);
  rotation_<T> Rx(vector_4_<T>(T(sin(half_x)), 0, 0, T(cos(half_x))));
  rotation_<T> Ry(vector_4_<T>(0, T(sin(half_y)), 0, T(cos(half_y))));
  rotation_<T> Rz(vector_4_<T>(0, 0, T(sin(half_z)), T(cos(half_z))));
  *this = Rx * Ry * Rz * Rned2enu;
}

/// Constructor - from a matrix
/**
 * requires orthonormal matrix with +1 determinant
 */
template <typename T>
rotation_<T>
::rotation_(const matrix_<3,3,T>& rot)
{
  // use double caclulations for more accuracy
  double d0 = rot(0,0), d1 = rot(1,1), d2 = rot(2,2);
  double xx = 1.0 + d0 - d1 - d2;
  double yy = 1.0 - d0 + d1 - d2;
  double zz = 1.0 - d0 - d1 + d2;
  double rr = 1.0 + d0 + d1 + d2;

  // determine the maximum term
  double max_v = rr;
  if (xx > max_v) max_v = xx;
  if (yy > max_v) max_v = yy;
  if (zz > max_v) max_v = zz;

  // choose the derivation that involves division by
  // the largest term, for stability
  if (rr == max_v) {
    T r4 = T(std::sqrt(rr)*2);
    q_[3] = r4 / 4;
    r4 = T(1) / r4;
    q_[0] = (rot(2,1) - rot(1,2)) * r4;
    q_[1] = (rot(0,2) - rot(2,0)) * r4;
    q_[2] = (rot(1,0) - rot(0,1)) * r4;
  }
  else if (xx == max_v) {
    T x4 = T(std::sqrt(xx)*2);
    q_[0] = x4 / 4;
    x4 = T(1) / x4;
    q_[1] = (rot(1,0) + rot(0,1)) * x4;
    q_[2] = (rot(2,0) + rot(0,2)) * x4;
    q_[3] = (rot(2,1) - rot(1,2)) * x4;
  }
  else if (yy == max_v) {
    T y4 = T(std::sqrt(yy)*2);
    q_[1] =  y4 / 4;
    y4 = T(1) / y4;
    q_[0] = (rot(1,0) + rot(0,1)) * y4;
    q_[2] = (rot(2,1) + rot(1,2)) * y4;
    q_[3] = (rot(0,2) - rot(2,0)) * y4;
  }
  else {
    T z4 = T(std::sqrt(zz)*2);
    q_[2] =  z4 / 4;
    z4 = T(1) / z4;
    q_[0] = (rot(2,0) + rot(0,2)) * z4;
    q_[1] = (rot(2,1) + rot(1,2)) * z4;
    q_[3] = (rot(1,0) - rot(0,1)) * z4;
  }
}

/// Convert to a 3x3 matrix
template <typename T>
rotation_<T>
::operator matrix_<3,3,T>() const
{
  T x2 = q_.x()*q_.x(), xy = q_.x()*q_.y(), rx = q_.w()*q_.x(),
    y2 = q_.y()*q_.y(), yz = q_.y()*q_.z(), ry = q_.w()*q_.y(),
    z2 = q_.z()*q_.z(), zx = q_.z()*q_.x(), rz = q_.w()*q_.z(),
    r2 = q_.w()*q_.w();
  matrix_<3,3,T> mat;
  // fill diagonal terms
  mat(0,0) = r2 + x2 - y2 - z2;
  mat(1,1) = r2 - x2 + y2 - z2;
  mat(2,2) = r2 - x2 - y2 + z2;
  // fill off diagonal terms
  mat(0,1) = 2 * (xy - rz);
  mat(0,2) = 2 * (zx + ry);
  mat(1,2) = 2 * (yz - rx);
  mat(1,0) = 2 * (xy + rz);
  mat(2,0) = 2 * (zx - ry);
  mat(2,1) = 2 * (yz + rx);
  return mat;
}

/// Returns the axis of rotation
template <typename T>
vector_3_<T>
rotation_<T>
::axis() const
{
  vector_3_<T> dir(q_.x(), q_.y(), q_.z());
  T mag = dir.magnitude();
  if (mag == T(0))
  {
    return vector_3_<T>(0,0,1);
  }
  return dir / mag;
}

/// Returns the angle of the rotation in radians about the axis
template <typename T>
T
rotation_<T>
::angle() const
{
  const double i = vector_3_<T>(q_.x(), q_.y(), q_.z()).magnitude();
  const double r = q_.w();
  T a = static_cast<T>(2.0 * std::atan2(i, r));
  const T pi = boost::math::constants::pi<T>();
  const T two_pi = static_cast<T>(2) * boost::math::constants::pi<T>();
  // make sure computed angle lies within a sensible range,
  // i.e. -pi/2 < a < pi/2
  if (a >= pi)
  {
    a -= two_pi;
  }
  if (a <= -pi)
  {
    a += two_pi;
  }
  return a;
}

/// Return the rotation as a Rodrigues vector
template <typename T>
vector_3_<T>
rotation_<T>
::rodrigues() const
{
  T angle = this->angle();
  if (angle == 0.0)
  {
    return vector_3_<T>(0,0,0);
  }
  return this->axis() * angle;
}

/// Convert to yaw, pitch, and roll
template <typename T>
void
rotation_<T>
::get_yaw_pitch_roll(T& yaw, T& pitch, T& roll) const
{
  matrix_<3,3,T> rotM(*this);
  T cos_p = T(std::sqrt(double(rotM(1,2)*rotM(1,2)) + rotM(2,2)*rotM(2,2)));
  yaw   = T(std::atan2(double(rotM(0,0)),double(rotM(0,1))));
  pitch = T(std::atan2(double(rotM(0,2)),double(cos_p)));
  roll  = T(std::atan2(double(-rotM(1,2)),double(-rotM(2,2))));
}

/// Compose two rotations
template <typename T>
rotation_<T>
rotation_<T>
::operator*(const rotation_<T>& rhs) const
{
  vector_<4,T> comp_q;
  const vector_<4,T>& q1 = this->q_;
  const vector_<4,T>& q2 = rhs.q_;
  comp_q[3] = q1[3]*q2[3] - q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2];
  comp_q[0] = q1[3]*q2[0] + q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1];
  comp_q[1] = q1[3]*q2[1] + q1[1]*q2[3] + q1[2]*q2[0] - q1[0]*q2[2];
  comp_q[2] = q1[3]*q2[2] + q1[2]*q2[3] + q1[0]*q2[1] - q1[1]*q2[0];
  return rotation_<T>(comp_q);
}

/// Rotate a vector
/**
 * \note for a large number of vectors, it is more efficient to
 * create a rotation matrix and use matrix multiplcation
 */
template <typename T>
vector_<3,T>
rotation_<T>
::operator*(const vector_<3,T>& rhs) const
{
  const T& real = q_.w();
  const vector_3_<T> imag(q_.x(), q_.y(), q_.z());
  const vector_<3,T> ixv(cross_product(imag, rhs));
  return rhs + T(2*real)*ixv - T(2)*cross_product(ixv,imag);
}

/// output stream operator for a rotation
template <typename T>
std::ostream&  operator<<(std::ostream& s, const rotation_<T>& r)
{
  s << r.quaternion();
  return s;
}

/// input stream operator for a rotation
template <typename T>
std::istream&  operator>>(std::istream& s, rotation_<T>& r)
{
  vector_4_<T> q;
  s >> q;
  r = rotation_<T>(q);
  return s;
}

/// Generate a rotation vector that, when applied to A N times, produces B.
template <typename T>
rotation_<T>
interpolate_rotation(rotation_<T> const& A, rotation_<T> const& B, T f)
{
  // rotation from A -> B
  rotation_<T> C = A.inverse() * B;
  // Reduce the angle of rotation by the fraction provided
  return A * rotation_<T>(C.angle() * f, C.axis());
}

/// Generate N evenly interpolated rotations inbetween \c A and \c B.
template <typename T>
void
interpolated_rotations(rotation_<T> const& A, rotation_<T> const& B, size_t n, std::vector< rotation_<T> > & interp_rots)
{
  interp_rots.reserve(interp_rots.capacity() + n);
  size_t denom = n + 1;
  for (size_t i=1; i<denom; ++i)
  {
    interp_rots.push_back(interpolate_rotation<T>(A, B, static_cast<T>(i) / denom));
  }
}

/// \cond DoxygenSuppress
#define INSTANTIATE_ROTATION(T) \
template class MAPTK_CORE_EXPORT rotation_<T>; \
template MAPTK_CORE_EXPORT std::ostream&  operator<<(std::ostream& s, const rotation_<T>& r); \
template MAPTK_CORE_EXPORT std::istream&  operator>>(std::istream& s, rotation_<T>& r); \
template MAPTK_CORE_EXPORT rotation_<T> interpolate_rotation(rotation_<T> const& A, rotation_<T> const& B, T f); \
template MAPTK_CORE_EXPORT void \
interpolated_rotations(rotation_<T> const& A, rotation_<T> const& B, size_t n, std::vector< rotation_<T> > & interp_rots)

INSTANTIATE_ROTATION(double);
INSTANTIATE_ROTATION(float);

#undef INSTANTIATE_ROTATION
/// \endcond

} // end namespace maptk


#define BOOST_TEST_MODULE dwDESS
#include <boost/test/unit_test.hpp>

#include <utility>

#include <sycomore/epg/Discrete.h>
#include <sycomore/epg/Regular.h>
#include <sycomore/Quantity.h>
#include <sycomore/Species.h>
#include <sycomore/units.h>

#include <boost/math/special_functions/relative_difference.hpp>

using namespace sycomore::units;

/*
 * Return the DESS signal with treatment of diffusion, cf. Freed et al.,
 * Steady-state free precession experiments and exact treatment of diffusion in
 * a uniform gradient. Journal of Chemical Physics 115(9), pp. 4249-4258. 2001.
 * doi:10.1063/1.1389859
 */
std::pair<double, double> freed(
    sycomore::Species const & species, sycomore::Quantity const & alpha,
    sycomore::Quantity const & G_tau, sycomore::Quantity const & TR,
    double M0=1, int truncation_level=6);

struct Fixture
{
    // This follows the DW-DESS parameters in Materials&Methods/Numerical
    // simulations of "Quantitative In Vivo Diffusion Imaging of Cartilage
    // Using Double Echo Steady-State Free Precession", Bieri & al., Magnetic
    // Resonance in Medicine 68(3), 2012.
    // doi:10.1002/mrm.23275

    sycomore::Species const species{1200_ms, 50_ms, 1_um*um/ms};

    sycomore::Quantity const flip_angle{20_deg};
    sycomore::Quantity const G_tau{200_mT/m*ms};
    sycomore::Quantity const TR{10_ms};

    std::pair<double, double> const analytical{
        freed(species, flip_angle, G_tau, TR)};
};

BOOST_FIXTURE_TEST_CASE(Regular, Fixture, *boost::unit_test::tolerance(1e-5))
{
    sycomore::epg::Regular model(species, {0,0,1}, 100, G_tau);
    std::pair<double, double> epg;
    unsigned int const repetitions = 5*species.get_T1()/TR;
    for(unsigned int repetition = 0; repetition != repetitions; ++repetition)
    {
        model.apply_pulse(flip_angle);
        epg.first = std::abs(model.echo());
        model.apply_time_interval(TR, G_tau/TR);
        epg.second = std::abs(model.echo());
    }

    BOOST_TEST(analytical.first == epg.first);
    BOOST_TEST(analytical.second == epg.second);
}

BOOST_FIXTURE_TEST_CASE(RegularThreshold, Fixture, *boost::unit_test::tolerance(1e-5))
{
    sycomore::epg::Regular model(species, {0,0,1}, 100, G_tau);
    model.threshold = 1e-6;

    std::pair<double, double> epg;
    unsigned int const repetitions = 5*species.get_T1()/TR;
    for(unsigned int repetition = 0; repetition != repetitions; ++repetition)
    {
        model.apply_pulse(flip_angle);
        epg.first = std::abs(model.echo());
        model.apply_time_interval(TR, G_tau/TR);
        epg.second = std::abs(model.echo());
    }

    BOOST_TEST(analytical.first == epg.first);
    BOOST_TEST(analytical.second == epg.second);
}

BOOST_FIXTURE_TEST_CASE(DiscreteNoReadout, Fixture, *boost::unit_test::tolerance(1e-5))
{
    sycomore::epg::Discrete model(species);
    std::pair<double, double> epg;
    unsigned int const repetitions = 5*species.get_T1()/TR;
    for(unsigned int repetition = 0; repetition != repetitions; ++repetition)
    {
        model.apply_pulse(flip_angle);
        epg.first = std::abs(model.echo());
        model.apply_time_interval(TR, G_tau/TR);
        epg.second = std::abs(model.echo());
    }

    BOOST_TEST(analytical.first == epg.first);
    BOOST_TEST(analytical.second == epg.second);
}

BOOST_FIXTURE_TEST_CASE(DiscreteThresholdNoReadout, Fixture, *boost::unit_test::tolerance(1e-5))
{
    sycomore::epg::Discrete model(species);
    model.threshold = 1e-6;

    std::pair<double, double> epg;
    unsigned int const repetitions = 5*species.get_T1()/TR;
    for(unsigned int repetition = 0; repetition != repetitions; ++repetition)
    {
        model.apply_pulse(flip_angle);
        epg.first = std::abs(model.echo());
        model.apply_time_interval(TR, G_tau/TR);
        epg.second = std::abs(model.echo());
    }

    BOOST_TEST(analytical.first == epg.first);
    BOOST_TEST(analytical.second == epg.second);
}

BOOST_FIXTURE_TEST_CASE(DiscreteWithReadout, Fixture, *boost::unit_test::tolerance(5e-5))
{
    // This follows the DW-DESS implementation in Fig. 2 of "Quantitative In
    // Vivo Diffusion Imaging of Cartilage Using Double Echo Steady-State Free
    // Precession", Bieri & al., Magnetic Resonance in Medicine 68(3), 2012.
    // doi:10.1002/mrm.23275
    // The signal will be be affected by
    // - The shift from start and end of TR (readout duration)
    // - The readout gradient area
    // Using unrealistic values for readout duration and voxel size as well as
    // correcting the diffusion time interval to include readout gradient allows
    // staying close to the analytical model.

    sycomore::Quantity const readout_duration = 0.1_us;
    sycomore::Quantity const voxel_size{10_cm};
    sycomore::TimeInterval const ro_plus{
        0.5*readout_duration, 0.5 * 2*M_PI/voxel_size};
    sycomore::TimeInterval const ro_minus{
        ro_plus.get_duration(), -ro_plus.get_gradient_amplitude()};
    sycomore::TimeInterval const diffusion{
        TR-6*readout_duration, G_tau - 2*ro_plus.get_gradient_area()[0]};

    sycomore::epg::Discrete model(species);
    std::pair<double, double> epg;
    unsigned int const repetitions = 5*species.get_T1()/TR;
    for(unsigned int repetition = 0; repetition != repetitions; ++repetition)
    {
        model.apply_pulse(flip_angle);

        model.apply_time_interval(ro_minus);
        model.apply_time_interval(ro_plus);
        epg.first = std::abs(model.echo());
        model.apply_time_interval(ro_plus);

        model.apply_time_interval(diffusion);

        model.apply_time_interval(ro_plus);
        epg.second = std::abs(model.echo());
        model.apply_time_interval(ro_plus);
        model.apply_time_interval(ro_minus);
    }

    BOOST_TEST(analytical.first == epg.first);
    BOOST_TEST(analytical.second == epg.second);
}

BOOST_FIXTURE_TEST_CASE(DiscreteThresholdWithReadout, Fixture, *boost::unit_test::tolerance(5e-5))
{
    sycomore::Quantity const readout_duration = 0.1_us;
    sycomore::Quantity const voxel_size{10_cm};
    sycomore::TimeInterval const ro_plus{
        0.5*readout_duration, 0.5 * 2*M_PI/voxel_size};
    sycomore::TimeInterval const ro_minus{
        ro_plus.get_duration(), -ro_plus.get_gradient_amplitude()};
    sycomore::TimeInterval const diffusion{
        TR-6*readout_duration, G_tau - 2*ro_plus.get_gradient_area()[0]};

    sycomore::epg::Discrete model(species);
    model.threshold = 1e-6;

    std::pair<double, double> epg;
    unsigned int const repetitions = 5*species.get_T1()/TR;
    for(unsigned int repetition = 0; repetition != repetitions; ++repetition)
    {
        model.apply_pulse(flip_angle);

        model.apply_time_interval(ro_minus);
        model.apply_time_interval(ro_plus);
        epg.first = std::abs(model.echo());
        model.apply_time_interval(ro_plus);

        model.apply_time_interval(diffusion);

        model.apply_time_interval(ro_plus);
        epg.second = std::abs(model.echo());
        model.apply_time_interval(ro_plus);
        model.apply_time_interval(ro_minus);
    }

    BOOST_TEST(analytical.first == epg.first);
    BOOST_TEST(analytical.second == epg.second);
}

std::pair<double, double> freed(
    sycomore::Species const & species, sycomore::Quantity const & alpha,
    sycomore::Quantity const & G_tau, sycomore::Quantity const & TR_,
    double M0, int truncation_level)
{
    using namespace sycomore::units;

    // Cast early to avoid repetitions in the continued fraction
    auto const R1 = species.get_R1().convert_to(Hz);
    auto const R2 = species.get_R2().convert_to(Hz);
    auto const TR = TR_.convert_to(s);

    auto const cos_alpha = std::cos(alpha.convert_to(rad));
    auto const sin_alpha = std::sin(alpha.convert_to(rad));

    // Diffusion rate, at the end of the first paragraph of p. 4251
    auto const R_D = (
            species.get_D()[0] * std::pow(sycomore::gamma*G_tau, 2)
        ).convert_to(Hz);

    // Equation 5, using definition of R_D
    auto const E1 = [&](int p) {
        return std::exp(-TR * (R1 + R_D * std::pow(p, 2))); };

    // Equation 4, using definition of R_D
    auto const E2 = [&](int p) {
        return std::exp(-TR * (R2 + R_D*(std::pow(p, 2) + p + 1./3.))); };

    // Equation 9
    auto const A = [&](int p) { return 0.5 * (E1(p) - 1.) * (1. + cos_alpha); };

    // Equation 10
    auto const B = [&](int p) { return 0.5 * (E1(p) + 1.) * (1. - cos_alpha); };

    // Equation 11
    auto const C = [&](int p) { return E1(p) - cos_alpha; };

    // Equation 24
    auto const n = [&](int p) {
        return -E2(-p) * E2(p-1) * std::pow(A(p), 2) * B(p-1) / B(p); };

    // Equation 25
    auto const d = [&](int p) {
        return A(p)-B(p) + E2(p) * E2(-p-1) * B(p) * C(p+1) / B(p+1); };

    // Equation 26 for p=l
    int l = truncation_level;
    auto const e_l = -E2(l) * E2(-l-1) * B(l) * C(l+1) / B(l+1);
    // Equation 23
    auto x_1 = n(truncation_level) / (d(truncation_level) + e_l);
    for(int l=truncation_level-1; l>0; --l)
    {
        x_1 = n(l) / (d(l)+x_1);
    }

    // Equation 22
    auto const r_1 = 1/(E2(-1) * B(0)) * x_1 + E2(0) * C(1) / B(1);

    // Equation 27, include relaxation term for b_{-1}
    auto const b_0 =
        -sin_alpha * M0 * (1 - E1(0)) / (A(0) - B(0) + r_1 * E2(-1) * C(0));
    auto const b_minus_1 = -r_1 * E2(-1) * b_0;

    // b_0 is the FID mode, b_{-1} is the pre-FID mode.
    return std::make_pair(b_0, -b_minus_1);
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the smallest representable value
    strictly greater than the parameter except:

    - for entries of floating types

       - next(Valmax) is Inf

       - next(-Inf) is Valmin

       - next(Inf) is Inf

       - next(Nan) is Nan

    - for entries of integral type

      - next(Valmax) is Valmin

    @par Header <boost/simd/function/next.hpp>

    @see nextafter, prev, successor, predecessor

    @par Example:

      @snippet next.cpp next

    @par Possible output:

      @snippet next.txt next

  **/
  Value next(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/next.hpp>
#include <boost/simd/function/simd/next.hpp>

#endif


/* Copyright 2018 Ignacio Torroba (ignaciotb@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef CORRESPONDENCECLASS_HPP
#define CORRESPONDENCECLASS_HPP

#include <ros/ros.h>
//#include "ekf_general/sensors_read.h"
//#include "ekf_general/plot_map.h"
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float32.h>
#include <tf/tf.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/operation_blocked.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <boost/scoped_ptr.hpp>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>

#include <vector>
#include <fstream>
#include <queue>
#include <math.h>
#include <algorithm>
#include <functional>
#include <list>
#include <iostream>
#include <cctype>

#include "utils_matrices/utils_matrices.hpp"

double angleLimit (double angle);

/**
 * @brief The CorrespondenceClass class
 * Auxiliar class containing all the info related to a correspondence
 * made between a landmark j and a measurement i at time t
 */
class CorrespondenceClass{

public:

    double psi_;
    double d_m_;
    boost::numeric::ublas::matrix<double> H_;
    boost::numeric::ublas::matrix<double> S_;
    boost::numeric::ublas::matrix<double> S_inverted_;
    boost::numeric::ublas::vector<int> landmark_pos_;
    boost::numeric::ublas::vector<double> nu_;
    int landmark_id_;

    CorrespondenceClass(const double &lm_id);
    /**
     * @brief computeH
     * @param mu_hat
     * @param lm_odom
     * Computes the jacobian of the measurment model
     */
    void computeH(const boost::numeric::ublas::vector<double> &mu_hat,
                  const tf::Vector3 lm_odom);
    /**
     * @brief computeS
     * @param sigma
     * @param Q
     * S = H*Q*H^T + Q
     */
    void computeS(const boost::numeric::ublas::matrix<double> &sigma, const boost::numeric::ublas::matrix<double> &Q);
    /**
     * @brief computeNu
     * @param z_hat_i
     * @param z_i
     * Computes the innovation
     */
    void computeNu(const boost::numeric::ublas::vector<double> &z_hat_i, const boost::numeric::ublas::vector<double> &z_i);
    /**
     * @brief computeLikelihood
     * Likelihood of the correspondence mj, zi
     * It also outputs the Mahalanobis distance between z_hat_i, z_i for outlier detection
     */
    void computeLikelihood();

private:
};

#endif // CORRESPONDENCECLASS_HPP


/* Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  * Neither the name of NVIDIA CORPORATION nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "workload/density-models/fixed-structured-distribution.hpp"

#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>

BOOST_CLASS_EXPORT(problem::FixedStructuredDistribution)

namespace problem{

FixedStructuredDistribution::FixedStructuredDistribution(){ }

FixedStructuredDistribution::FixedStructuredDistribution(const Specs& specs) : specs_(specs){
  is_specced_ = true;
  workload_tensor_size_set_ = false;
}

FixedStructuredDistribution::~FixedStructuredDistribution() { }

FixedStructuredDistribution::Specs FixedStructuredDistribution::ParseSpecs(config::CompoundConfigNode density_config){

  Specs specs;
  std::string type;
  double fixed_density;

  density_config.lookupValue("distribution", type);
  if( !density_config.lookupValue("density", fixed_density))
  {
    std::cerr << "ERROR: missing density value specification for " << type << " distribution" << std::endl;
    exit(1);
  }

  specs.type = "fixed-structured";
  specs.fixed_density = fixed_density;

  return specs;
}

void FixedStructuredDistribution::SetWorkloadTensorSize(const PointSet& point_set)
{
  specs_.workload_tensor_size = point_set.size();
  workload_tensor_size_set_ = true;
}

std::uint64_t FixedStructuredDistribution::GetTileOccupancyByConfidence(const std::uint64_t tile_shape,
                                                              const double confidence)
{

  double exact_nnzs = tile_shape * specs_.fixed_density;
  // now we need to decide how to assign the prob of ceil and floor so that the expected nnzs == exact_nnzs

  // assume prob of ceil is pc, then the prob of floor is pf = 1 - pc
  //  => pc*ceil + (1-pc)*floor = exact_nnzs
  //  => pc = exact_nnzs-floor

  double prob_ceil = exact_nnzs - floor(exact_nnzs);
  double prob_floor = 1 - prob_ceil;

  return confidence <= prob_floor ? floor(exact_nnzs) : ceil(exact_nnzs);
}

std::uint64_t FixedStructuredDistribution::GetMaxNumElementByConfidence(const tiling::CoordinateSpaceTileInfo& fiber_tile,
                                                                        const tiling::CoordinateSpaceTileInfo& element_tile,
                                                                        const double confidence)
{
  std::uint64_t element_shape = element_tile.GetShape();
  std::uint64_t fiber_shape = fiber_tile.GetShape();

  //  std::cout << "fiber shape: " << fiber_shape << " element shape: " << element_shape << std::endl;

  // Sanity check
  if (fiber_shape % element_shape != 0)
  {
    std::cerr << "ERROR: fiber shape not divisiable by element shape" << std::endl;
    assert(false);
  }

  std::uint64_t num_elements = fiber_shape / element_shape;

  // Maximum number of nonzeros in the entire fiber
  std::uint64_t max_fiber_occupancy = GetMaxTileOccupancyByConfidence(fiber_tile, confidence);

  // Nonzeros are evenly distributed in the elements
  return max_fiber_occupancy > num_elements ? num_elements : max_fiber_occupancy;

}

std::uint64_t FixedStructuredDistribution::GetMaxTileOccupancyByConfidence(const tiling::CoordinateSpaceTileInfo& tile,
                                                                 const double confidence)
{
  std::uint64_t tile_shape = tile.GetShape();
  return FixedStructuredDistribution::GetTileOccupancyByConfidence(tile_shape, confidence);
}

// place holder lightweight version
std::uint64_t FixedStructuredDistribution::GetMaxTileOccupancyByConfidence_LTW (const std::uint64_t tile_shape,
                                                   const double confidence)
{
  return FixedStructuredDistribution::GetTileOccupancyByConfidence(tile_shape, confidence);
}

std::uint64_t FixedStructuredDistribution::GetWorkloadTensorSize() const
{
  assert(workload_tensor_size_set_);
  return specs_.workload_tensor_size;
}

std::string FixedStructuredDistribution::GetDistributionType() const{
  return specs_.type;
}

double FixedStructuredDistribution::GetMaxTileDensityByConfidence(const tiling::CoordinateSpaceTileInfo tile,
                                                                  const double confidence)
{
  (void) confidence;

  if (tile.GetShape() == 0) return 0;
  return specs_.fixed_density;
}

double FixedStructuredDistribution::GetMinTileDensity(const tiling::CoordinateSpaceTileInfo tile)
{
  if (tile.GetShape() == 0)
  {
    return 0;
  }
  std::uint64_t floor_nnzs =  floor(tile.GetShape() * specs_.fixed_density);
  return (double)floor_nnzs/tile.GetShape();
}

double FixedStructuredDistribution::GetTileOccupancyProbability(const tiling::CoordinateSpaceTileInfo& tile,
                                                                const std::uint64_t occupancy)
{
  assert(is_specced_);
  assert(workload_tensor_size_set_);
  double prob, exact_nnzs;
  std::uint64_t tile_shape = tile.GetShape();
  assert(tile.mold_set_);

  if (tile.HasExtraConstraintInfo())
  {
    auto extra_constraint_info = tile.GetExtraConstraintInfo();
    double constrained_density = (double)extra_constraint_info.GetOccupancy()/extra_constraint_info.GetShape();
    exact_nnzs = tile_shape * constrained_density;
  }
  else
  {
    exact_nnzs = tile_shape * specs_.fixed_density;
  }

   // now we need to decide how to assign the prob of ceil and floor so that the expected nnzs == exact_nnzs

  // assume prob of ceil is pc, then the prob of floor is pf = 1 - pc
  //  => pc*ceil + (1-pc)*floor = exact_nnzs
  //  => pc = exact_nnzs-floor

  double prob_ceil = exact_nnzs - floor(exact_nnzs);
  double prob_floor = 1 - prob_ceil;

  if (floor(exact_nnzs) == occupancy)
  {
    return prob_floor;

  } else if (ceil(exact_nnzs) == occupancy)
  {
    return prob_ceil;

  } else
  {
    return 0.0;
  }

  return prob;
}

double FixedStructuredDistribution::GetExpectedTileOccupancy (const tiling::CoordinateSpaceTileInfo tile)
{
  return tile.GetShape() * specs_.fixed_density;
}

}


#ifndef MAP2CAT_HPP
#define MAP2CAT_HPP

#include <fstream>
#include <iostream>
#include <iomanip>
#include <exception>
#include <vector>
#include <cmath>
#include <cassert>
#include <random>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/log/trivial.hpp>
#include <boost/tokenizer.hpp>

#include <healpix_base.h>
#include <healpix_map.h>
#include <pointing.h>
#include <healpix_map_fitsio.h>
#include <datatypes.h>

class Map2Cat
{
public:

    typedef boost::property_tree::ptree propertyTreeType;
    typedef Healpix_Map<double> mapType;
    static const constexpr double deg2rad = M_PI/double(180);
    static const constexpr double rotPhi = double(0);

    explicit Map2Cat(std::string const& iniFileName)
    {
        BOOST_LOG_TRIVIAL(info) << std::string("Reading the ini file ") + std::string(iniFileName);
        boost::property_tree::ini_parser::read_ini(iniFileName,mPropTree);

        std::string inputMapFileName = mPropTree.get<std::string>("input.data_map_file_name");
        BOOST_LOG_TRIVIAL(info) << std::string("Reading the maps from ") + inputMapFileName;
        read_Healpix_map_from_fits(inputMapFileName,mMapN);
        read_Healpix_map_from_fits(inputMapFileName,mMapE1,int(2),int(2));
        read_Healpix_map_from_fits(inputMapFileName,mMapE2,int(3),int(2));
    }

    void generate()
    {
        std::string zBoundsStr = mPropTree.get<std::string>("input.z_bounds");
        BOOST_LOG_TRIVIAL(info) << "z bounds specified as "<< zBoundsStr;
        typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
        boost::char_separator<char> sep(",");
        tokenizer tokens(zBoundsStr, sep);

        for(tokenizer::iterator tokIter = tokens.begin(); tokIter !=tokens.end(); ++tokIter)
        {
            mZBounds.push_back(boost::lexical_cast<double>(*tokIter));
        }

        if(mZBounds.size() != size_t(2))
        {
            std::string msg("The z-bounds should consist of two values. No more, no less.");
            throw std::runtime_error(msg);
        }

        if(mZBounds[0] >= mZBounds[1])
        {
            std::string msg("The upper bound should be greater than the lower bound.");
            throw std::runtime_error(msg);
        }

        size_t numPix = (size_t) mMapN.Npix();

        size_t randomSeed = mPropTree.get<size_t>("input.rand_seed");
        BOOST_LOG_TRIVIAL(info) << "Random seed specified as "<< randomSeed;

        std::mt19937 gen(randomSeed);

        // to generate z values
        std::uniform_real_distribution<double> dist_z(mZBounds[0], mZBounds[1]);
        // to generate e1 and e2 values
        std::normal_distribution<> dist_e(0,1);

        double sigma_e = mPropTree.get<double>("input.sigma_e");
        BOOST_LOG_TRIVIAL(info) << "Std-dvn of ellipticities specified as "<< sigma_e;

        BOOST_LOG_TRIVIAL(info) << "Number of pixel in the map is "<< numPix;

        std::ofstream inputCatFile;
        std::string inputCatFileName = mPropTree.get<std::string>("output.catlogue_file_name");
        BOOST_LOG_TRIVIAL(info) << "Output catalogue file name is "<< inputCatFileName;

        inputCatFile.open( inputCatFileName.c_str(),std::ios::trunc );

        //write the header
        std::string delimiter = mPropTree.get<std::string>("output.delimiter");
        BOOST_LOG_TRIVIAL(info) << "Delimiter for separation is "<< delimiter;

        inputCatFile<<"#"<<"ra"<<delimiter<<"dec"<<delimiter<<"z"
            <<delimiter<<"e1"<<delimiter<<"e2"<<std::endl;

        inputCatFile<<std::setprecision(10);
        // go through the number count map and generate n uniform random from zmin and zmax
        for(size_t i=0;i<numPix;++i)
        {
            size_t numGals = (size_t) mMapN[i];
            double e1 = mMapE1[i];
            double e2 = mMapE2[i];

            pointing pntg = mMapN.pix2ang(i);
            double theta = pntg.theta;
            double phi = pntg.phi;

            double dec = -( theta - M_PI*double(0.5) )/deg2rad;
            double ra = phi/deg2rad + rotPhi;

            for(size_t j=0;j<numGals;++j)
            {
                double zVal = dist_z(gen);
                double e1Val = e1 + dist_e(gen)*sigma_e;
                double e2Val = e2 + dist_e(gen)*sigma_e;
                inputCatFile<<ra<<delimiter<<dec<<delimiter<<zVal<<delimiter<<e1Val<<delimiter<<e2Val<<std::endl;
            }
        }

        inputCatFile.close();
    }

private:
    propertyTreeType mPropTree;
    mapType mMapN;
    mapType mMapE1;
    mapType mMapE2;
    std::vector<double> mZBounds;

};

#endif //MAP2CAT_HPP


/*
 * rk_performance_test_case.hpp
 *
 * Copyright 2009-2012 Karsten Ahnert
 * Copyright 2009-2012 Mario Mulansky
 *
 * 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)
 */

#include <iostream>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/timer.hpp>

#define tab "\t"

using namespace std;
using namespace boost::accumulators;

typedef accumulator_set<
    double , stats< tag::mean , tag::variance >
    > accumulator_type;

ostream& operator<<( ostream& out , accumulator_type &acc )
{
    out << boost::accumulators::mean( acc ) << tab;
//    out << boost::accumulators::variance( acc ) << tab;
    return out;
}

typedef boost::timer timer_type;

template< class Stepper >
void run( Stepper &stepper , const size_t num_of_steps = 20000000 , const double dt = 1E-10 )
{
    const size_t loops = 20;

    accumulator_type acc;
    timer_type timer;

    srand( 12312354 );

    // transient
    //stepper.reset_init_cond( );
    //for( size_t i = 0 ; i < num_of_steps ; ++i )
    //    stepper.do_step( dt );

    for( size_t n=0 ; n<loops+1 ; ++n )
    {
        stepper.reset_init_cond( );

        timer.restart();
        for( size_t i = 0 ; i < num_of_steps ; ++i )
            stepper.do_step( dt );
        if( n>0 )
        {   // take first run as transient
            acc(timer.elapsed());
            clog.precision(8);
            clog.width(10);
            clog << acc << " " << stepper.state(0) << endl;
        }
    }
    cout << acc << endl;
}


#pragma once

#include <boost/assert.hpp>

#include "basis/spectral_function/spectral_spherical_real.hpp"
#include "mpfr/import_std_math.hpp"

#include <cmath>

namespace boltzmann {

/**
 *  @brief Evaluator wrapper for Polar-Laguerre angular basis fuctions.
 *
 *  Evaluates \f$ sin(k x), cos(
 k x) \f$ for a positive integer k.
 *
 */
template <typename NUMERIC>
class FTEval
{
 public:
  typedef NUMERIC numeric_t typedef XiR elem_t;

 public:
  FTEval();

  void compute(const numeric_t* x, unsigned int n);
  void compute(std::vector<numeric>&& x);

  numeric_t operator()(const elem_t& elem, unsigned int q)
  {
    BOOST_ASSERT(q < x_.size());

    if (elem.id().t == TRIG::COS)
      return ::math::cos(elem.id().l * x_[q]);
    else if (elem.id().t == TRIG::SIN) {
      return ::math::sin(elem.id().l * x_[q]);
    }
#ifdef DEBUG
    else {
      assert(false);
    }
#endif
  }

 private:
  std::vector<numeric_t> x_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC>
FTEval<NUMERIC>::compute(const numeric_t* x, unsigned int n)
{
  x_.resize(n);
  std::copy(x, x + n, x_.begin());
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
FTEval<NUMERIC>::compute(std::vector<numeric_t>&& x)
{
  x_ = std::forward(x);
}

}  // boltzmann


#include <HElib/FHE.h>
#include <HElib/FHEContext.h>
#include <HElib/EncryptedArray.h>
#include <HElib/NumbTh.h>

#include "SMP/Matrix.hpp"
#include "SMP/Timer.hpp"
#include "SMP/literal.hpp"
#include "SMP/network/net_io.hpp"

#include <boost/asio.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <iostream>
#include <numeric>
#include <list>
using boost::asio::ip::tcp;
constexpr long REPEATS = 10;

struct EncVec {
	long length;
	std::vector<Ctxt> ctxts;
};

void pack_vector(std::vector<NTL::ZZX> &out, Matrix const& mat, long COL, EncryptedArray const* ea)
{
	const long ROWS = mat.NumRows();
	long num_polys = (ROWS + ea->size() - 1) / ea->size();
	long itr = 0;
	out.resize(num_polys);
	for (long i = 0; i < num_polys; i++) {
		std::vector<long> slots(ea->size(), 0);
		for (long j = 0; j < ea->size() and itr < ROWS; j++) {
			slots[j] = mat[itr++][COL];
		}
		ea->encode(out[i], slots);
	}
}

void rotate_vector(Matrix &vec, long offset)
{
	assert(vec.NumCols() == 1);
	long length = vec.NumRows();
	while (offset < 0)
		offset += length;
	if (offset == 0)
		return;
	Matrix tmp;
	tmp.SetDims(vec.NumRows(), 1);
	for (long i = 0; i < length; i++)
		tmp[i][0] = vec[(i + offset) % length][0];
	vec = tmp;
}

void encrypt_vector(EncVec &out,
					Matrix const& vec,
					FHESecKey const& key,
					double *pack_time = nullptr)
{
	assert(vec.NumCols() == 1);
	FHEcontext const& context= key.getContext();
	EncryptedArray const* ea = context.ea;
	out.length = vec.NumRows();
	std::vector<NTL::ZZX> polys;
	{
		AutoTimer timer(pack_time);
		pack_vector(polys, vec, 0, ea);
	}
	out.ctxts.resize(polys.size(), Ctxt(key));
	for (size_t i = 0; i < polys.size(); i++) {
		key.Encrypt(out.ctxts[i], polys.at(i));
	}
}

void columns_to_vector(Matrix &vec, Matrix const&mat, long from, long to)
{
	vec.SetDims((to - from) * mat.NumRows(), 1);
	for (long j = from; j < to; j++) {
		long offset = (j - from) * mat.NumRows();
		for (long i = 0; i < mat.NumRows(); i++) {
			vec[offset + i][0] = mat[i][j];
		}
	}
}

void rows_to_vector(Matrix &vec, Matrix const&mat, long from, long to)
{
	assert(from >= 0 && to > from && to <= mat.NumRows());
	const long length = (to - from) * mat.NumCols();
	vec.SetDims(length, 1);
	for (long i = from; i < to; i++) {
		long offset = (i - from) * mat.NumCols();
		for (long j = 0; j < mat.NumCols(); j++) {
			vec[offset + j][0] = mat[i][j];
		}
	}
}

/// To fully pack the matrix, we concat each row of matrix into a long vector.
void encrypt_matrix_as_vector(EncVec &out,
							  Matrix const& mat,
							  FHESecKey const& key,
							  double *pack_time = nullptr)
{
	Matrix vec;
	rows_to_vector(vec, mat, 0, mat.NumRows());
	encrypt_vector(out, vec, key, pack_time);
}

void randomize(Matrix &mat) {
	for (long i = 0; i < mat.NumRows(); i++)
		for (long j = 0; j < mat.NumCols(); j++)
			mat[i][j] = NTL::RandomBnd(10L);
}

void mat_mult(std::list<Ctxt> &out,
			  EncVec const& enc_mat,
			  Matrix const& mat,
			  EncryptedArray const *ea)
{
	const auto row_cnt = mat.NumRows();
	const auto col_cnt = mat.NumCols();
	long cols_per_ctxt = ea->size() / row_cnt;
	assert(cols_per_ctxt >= 1 && "Current version only work for n2 <= ea->size()");
	for (long col = 0; col < col_cnt; col += cols_per_ctxt) {
		long cols_to_pack = std::min(col_cnt - col, cols_per_ctxt);
		Matrix vec;
		columns_to_vector(vec, mat, col, col + cols_to_pack);

		Matrix rnd;
		rnd.SetDims(cols_to_pack * row_cnt, 1);
		for (long rot = 0; rot < cols_to_pack; rot++) { // rotatiton
			long offset = rot * row_cnt;
			rotate_vector(vec, offset);
			std::vector<NTL::ZZX> polys;
			pack_vector(polys, vec, 0, ea); // pack the 0-th column
			assert(polys.size() == 1);

			for (const auto &ctx : enc_mat.ctxts) {
				Ctxt tmp(ctx);
				tmp.multByConstant(polys[0]); // multiplication

                randomize(rnd);
				std::vector<NTL::ZZX> rnd_poly;
				pack_vector(rnd_poly, rnd, 0, ea);
				assert(rnd_poly.size() == 1);
                tmp.addConstant(rnd_poly[0]); // adding the random share

				out.push_back(std::move(tmp));
			}
		}
	}
}

struct ClientBenchmark {
    std::vector<double> pack_times;
	std::vector<double> enc_times;
    std::vector<double> unpack_times;
	std::vector<double> dec_times;
    std::vector<double> total_times;
    int ctx_sent, ctx_recv;
};
ClientBenchmark clt_ben;

struct ServerBenchmark {
    std::vector<double> eval_times;
};
ServerBenchmark srv_ben;

void play_client(tcp::iostream &conn,
                 FHESecKey &sk,
                 FHEcontext &context,
                 const long n1,
                 const long n2,
                 const long n3)
{
    sk.convertToSymmetric();
	FHEPubKey ek(sk);
    conn << ek; // send evaluation key
    const EncryptedArray *ea = context.ea;
    const long l = ea->size();
    const long d = ea->getDegree();

    NTL::SetSeed(NTL::to_ZZ(123)); //use fixed seed for debugging
    Matrix A, B;
    A.SetDims(n1, n2);
    B.SetDims(n2, n3);
    randomize(A);
    randomize(B);

	EncVec enc_vec;
	double pack_time;
	double enc_time;
	do {
		AutoTimer timer(&enc_time);
		encrypt_matrix_as_vector(enc_vec, A, sk, &pack_time);
	} while(0);
	clt_ben.pack_times.push_back(pack_time);
	clt_ben.enc_times.push_back(enc_time);

    conn << static_cast<int64_t>(enc_vec.ctxts.size()) << std::endl;
    for (const auto &ctx : enc_vec.ctxts)
		conn << ctx;
    clt_ben.ctx_sent = enc_vec.ctxts.size();

	int64_t result_ctx_cnt;
	conn >> result_ctx_cnt;
    clt_ben.ctx_recv = result_ctx_cnt;
    std::vector<Ctxt> result(result_ctx_cnt, Ctxt(ek));
	for (size_t i = 0; i < result_ctx_cnt; i++)
		conn >> result[i];
    double eval_time = 0.;
    conn >> eval_time;
    srv_ben.eval_times.push_back(eval_time);

	double dec_time;
	double unpack_time;
	std::vector<long> slots;
	NTL::ZZX decrypted;
	for (const auto &ctx : result) {
		double one_dec_time;
		double one_unpack_time;
		{
			AutoTimer timer(&one_dec_time);
            if (!ctx.isCorrect())
				std::cerr << "decryption might fail" << std::endl;
			sk.Decrypt(decrypted, ctx);
		}
		{
			AutoTimer timer(&one_unpack_time);
            ea->decode(slots, decrypted);
		}
		dec_time += one_dec_time;
		unpack_time += one_unpack_time;
	}
	clt_ben.dec_times.push_back(dec_time);
	clt_ben.unpack_times.push_back(unpack_time);
}

void play_server(tcp::iostream &conn,
                 const long n1,
                 const long n2,
                 const long n3) {
    FHEcontext context = receive_context(conn);
	FHEPubKey evk(context);
	conn >> evk;

	NTL::SetSeed(NTL::to_ZZ(123)); //use fixed seed for debugging

    Matrix A, B;
    A.SetDims(n1, n2);
    B.SetDims(n2, n3);
    randomize(A);
    randomize(B);

	int64_t ctx_cnt;
	conn >> ctx_cnt;
	EncVec enc_vec;
	enc_vec.ctxts.resize(ctx_cnt, Ctxt(evk));
	for (size_t i = 0; i < ctx_cnt; i++) // receive ciphertexts from the client.
		conn >> enc_vec.ctxts[i];

	double eval_time;
	std::list<Ctxt> result;
	{
		AutoTimer timer(&eval_time);
		mat_mult(result, enc_vec, B, context.ea);
	}
	srv_ben.eval_times.push_back(eval_time);

    conn << static_cast<int64_t>(result.size()) << std::endl;
    for (const auto &ctx : result) // sending result back
		conn << ctx;
    conn << eval_time; // send back eval time for statistics
}

int run_client(std::string const& addr, long port,
			   long n1, long n2, long n3) {
    const long m = 8192;
    const long p = 65537;
    const long r = 1;
    const long L = 3;
    NTL::zz_p::init(p);
    FHEcontext context(m, p, r);
    context.bitsPerLevel = 20; // trial and error to find this value.
    buildModChain(context, L);
    std::cerr << "kappa = " << context.securityLevel() << std::endl;
    std::cerr << "slot = " << context.ea->size() << std::endl;
    std::cerr << "degree = " << context.ea->getDegree() << std::endl;
    FHESecKey sk(context);
    sk.GenSecKey(64);
    for (long t = 0; t < REPEATS; t++) {
        tcp::iostream conn(addr, std::to_string(port));
        if (!conn) {
            std::cerr << "Can not connect to server!" << std::endl;
            return -1;
        }
        /// send FHEcontext obj
        send_context(conn, context);
        double all_time;
        do {
            AutoTimer time(&all_time);
            play_client(conn, sk, context, n1, n2, n3);
        } while(0);
        clt_ben.total_times.push_back(all_time);
        conn.close();
    }
    return 1;
}

int run_server(long port, long n1, long n2, long n3) {
    boost::asio::io_service ios;
    tcp::endpoint endpoint(tcp::v4(), port);
    tcp::acceptor acceptor(ios, endpoint);

    for (long run = 0; run < REPEATS; run++) {
        tcp::iostream conn;
        boost::system::error_code err;
        acceptor.accept(*conn.rdbuf(), err);
        if (!err) {
            play_server(conn, n1, n2, n3);
        }
    }
    return 0;
}

int main(int argc, char *argv[]) {
    ArgMapping argmap;
    long role = -1;
    long n1 = 8;
    long n2 = 8;
    long n3 = 8;
    std::string addr = "127.0.0.1";
	long port = 12345;
    argmap.arg("N", n1, "n1");
    argmap.arg("M", n2, "n2");
    argmap.arg("D", n3, "n3");
    argmap.arg("R", role, "role. 0 for server and 1 for client");
	argmap.arg("a", addr, "server address");
	argmap.arg("p", port, "port");
    argmap.parse(argc, argv);
    if (role == 0) {
        run_server(port, n1, n2, n3);
    } else if (role == 1) {
        int st = run_client(addr, port, n1, n2, n3);
		auto times = mean_std(clt_ben.pack_times);
		printf("%.3f %.3f ", times.first, times.second);

		times = mean_std(clt_ben.enc_times);
		printf("%.3f %.3f ", times.first, times.second);

        times = mean_std(clt_ben.dec_times);
        printf("%.3f %.3f ", times.first, times.second);

		times = mean_std(clt_ben.unpack_times);
		printf("%.3f %.3f ", times.first, times.second);

        times = mean_std(clt_ben.total_times);
        printf("%.3f %.3f ", times.first, times.second);

        times = mean_std(srv_ben.eval_times);
        printf("%.3f %.3f ", times.first, times.second);
        printf("%d %d\n", clt_ben.ctx_sent, clt_ben.ctx_recv);
    } else {
		argmap.usage("General Matrix Multiplication for |N*M| * |M*D|");
		return -1;
	}
}

#if 0
int main(int argc, char *argv[]) {
	ArgMapping argmap;
    long n1 = 8;
    long n2 = 8;
	long n3 = 8;
    argmap.arg("N", n1, "n1");
    argmap.arg("M", n2, "n2");
    argmap.arg("K", n3, "n3");
    argmap.parse(argc, argv);

	Matrix A, B;
	A.SetDims(n1, n2);
	randomize(A);
	B.SetDims(n2, n3);
	randomize(B);
	Matrix AB = mul(A, B);

	FHEcontext context(8192, 8191, 1);
	std::cout << "slots " << context.ea->size() << std::endl;
	buildModChain(context, 4);
	std::cout << "security level " << context.securityLevel() << std::endl;
	FHESecKey sk(context);
	sk.GenSecKey(64);
	auto ea = context.ea;
	EncVec enc_vec;
	double pack_time;
	double enc_time;
	{
		AutoTimer timer(&enc_time);
		encrypt_matrix_as_vector(enc_vec, A, sk, &pack_time);
	}

	std::list<Ctxt> result;
	double eval_time;
	{
		AutoTimer timer(&eval_time);
		mat_mult(result, enc_vec, B, ea);
	}
	double dec_time;
	{
		AutoTimer timer(&dec_time);
		std::vector<long> slots;
		for (auto &ctx : result) {
			ea->decrypt(ctx, sk, slots);
		}
	}
	printf("%.3f %.3f %.3f %zd %zd\n",
		   enc_time, dec_time, eval_time,
		   enc_vec.ctxts.size(), result.size());
	return 0;
}
#endif


/*
Brian Staber (brian.staber@gmail.com)
*/

#include "Epetra_ConfigDefs.h"
#ifdef HAVE_MPI
#include "mpi.h"
#include "Epetra_MpiComm.h"
#else
#include "Epetra_SerialComm.h"
#endif

#include "nrl_PCA_Likelihood.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/special_functions/gamma.hpp>

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

    std::string xmlInFileName = "";

    Teuchos::CommandLineProcessor  clp(false);
    clp.setOption("xml-in-file",&xmlInFileName,"The XML file to read into a parameter list");
    clp.setDocString("Compilation: make -f Makefile.os\n"
                     "Run: mpirun -np 28 ./trilinos_mpi --xml-in-file='nrl.aztec.linux.xml'");

    Teuchos::CommandLineProcessor::EParseCommandLineReturn
    parse_return = clp.parse(argc,argv);
    if( parse_return != Teuchos::CommandLineProcessor::PARSE_SUCCESSFUL ) {
        std::cout << "\nEnd Result: TEST FAILED" << std::endl;
        return parse_return;
    }

#ifdef HAVE_MPI
    MPI_Init(&argc, &argv);
    Epetra_MpiComm Comm(MPI_COMM_WORLD);
#else
    Epetra_SerialComm Comm;
#endif

    Teuchos::RCP<Teuchos::ParameterList> paramList = Teuchos::rcp(new Teuchos::ParameterList);
    if(xmlInFileName.length()){
        Teuchos::updateParametersFromXmlFile(xmlInFileName, inoutArg(*paramList));
    }

    /*if (Comm.MyPID()==0){
        std::cout << "****************\n";
        std::cout << "Hyperparameters:\n";
        std::cout << paramList->sublist("TIMooney");
        std::cout << paramList->sublist("Shinozuka");
        std::cout << "****************\n";
    }*/

      Teuchos::RCP<nrl_PCA_Likelihood> RG =
      Teuchos::rcp(new nrl_PCA_Likelihood(Comm,*paramList));

      Epetra_IntSerialDenseVector seeds(5);
      Epetra_SerialDenseVector    mean_parameters(5);
      Epetra_SerialDenseVector    exponents(2);
      Epetra_SerialDenseVector    correlation_lengths(2);
      Epetra_SerialDenseVector    coeff_of_variation(4);
      Epetra_SerialDenseVector    plyagls(4);

      //mean values of the random parameters G_1(x),...,G_5(x)
      mean_parameters(0) = Teuchos::getParameter<double>(paramList->sublist("TIMooney"),"mu1");
      mean_parameters(1) = Teuchos::getParameter<double>(paramList->sublist("TIMooney"),"mu2");
      mean_parameters(2) = Teuchos::getParameter<double>(paramList->sublist("TIMooney"),"mu3");
      mean_parameters(3) = Teuchos::getParameter<double>(paramList->sublist("TIMooney"),"mu4");
      mean_parameters(4) = Teuchos::getParameter<double>(paramList->sublist("TIMooney"),"mu5");
      //mean_parameters.Scale(1.0e3);
      //deterministic exponents beta_4 and beta_5
      exponents(0) = Teuchos::getParameter<double>(paramList->sublist("TIMooney"),"beta4");
      exponents(1) = Teuchos::getParameter<double>(paramList->sublist("TIMooney"),"beta5");
      //correlation lengths of the Gaussian random field
      correlation_lengths(0) = Teuchos::getParameter<double>(paramList->sublist("Shinozuka"),"lx");
      correlation_lengths(1) = Teuchos::getParameter<double>(paramList->sublist("Shinozuka"),"ly");
      //coefficients of variation of the random parameters G_1(x),...,G_4(x)
      coeff_of_variation(0) = Teuchos::getParameter<double>(paramList->sublist("Shinozuka"),"delta1");
      coeff_of_variation(1) = Teuchos::getParameter<double>(paramList->sublist("Shinozuka"),"delta2");
      coeff_of_variation(2) = Teuchos::getParameter<double>(paramList->sublist("Shinozuka"),"delta3");
      coeff_of_variation(3) = Teuchos::getParameter<double>(paramList->sublist("Shinozuka"),"delta4");
      //ply angle
      plyagls(0) = 15.0;
      plyagls(1) = 30.0;
      plyagls(2) = 60.0;
      plyagls(3) = 75.0;

      int nmc = Teuchos::getParameter<int>(paramList->sublist("Shinozuka"),"nmc");

      for (unsigned int i=0; i<4; ++i){
        for (unsigned int j=0; j<nmc; ++j){
          seeds(0) = 5*(j+i*nmc)+0;
          seeds(1) = 5*(j+i*nmc)+1;
          seeds(2) = 5*(j+i*nmc)+2;
          seeds(3) = 5*(j+i*nmc)+3;
          seeds(4) = 5*(j+i*nmc)+4;
          int flag = RG->rnd(j                  ,
                             seeds              ,
                             mean_parameters    ,
                             exponents          ,
                             correlation_lengths,
                             coeff_of_variation ,
                             plyagls(i)         ,
                             false              ,
                             false              ,
                             false              ,
                             true               ,
                             false              );
        }
      }

#ifdef HAVE_MPI
    MPI_Finalize();
#endif
return 0;
}


#ifndef REVOLVE_DIFFERENTIAL_CPG_2_HPP
#define REVOLVE_DIFFERENTIAL_CPG_2_HPP

#include "Controller.h"

#include <vector>

#include "actuators/Actuator.h"
#include <boost/numeric/odeint.hpp>

namespace revolve
{

    class DifferentialCPG2 : public Controller
    {
    public:
        struct Parameters
        {
            double initial_neuron_state;
            double output_signal_gain;
            double abs_output_bound;
            std::vector<double> weights;
        };

        DifferentialCPG2(Parameters const &parameters, std::vector<std::shared_ptr<Actuator>> const &actuators);

        void update(
            std::vector<std::shared_ptr<Actuator>> const &actuators,
            std::vector<std::shared_ptr<Sensor>> const &sensors,
            double const time,
            double const dt) override;

    private:
        // indices of neurons corresponding to actuators
        std::vector<size_t> actuator_neurons;

        std::vector<double> neuron_state;

        std::vector<std::vector<double>> ode_matrix;

        // neural network ode solver
        boost::numeric::odeint::runge_kutta4<std::vector<double>> ode_solver;

        void create_neurons(Parameters const &parameters, std::vector<std::shared_ptr<Actuator>> const &actuators);
        void create_ode_matrix(Parameters const &parameters, std::vector<std::shared_ptr<Actuator>> const &actuators);
        void integrate_neural_network(double const time, double const dt);
        void update_actuators(std::vector<std::shared_ptr<Actuator>> const &actuators, double const dt);
    };

}

#endif // REVOLVE_DIFFERENTIAL_CPG_2_HPP


#include <armadillo>

#include "gtest/gtest.h"
#include "src/common/runner/Runner.h"

size_t size_of_matrix_without_degeneracy(const Matrix& matrix) {
    size_t accumulator = 0;
    for (const auto& submatrix : matrix.blocks) {
        accumulator += submatrix.raw_data.size();
    }
    return accumulator;
}

size_t size_of_matrix_with_degeneracy(const Matrix& matrix) {
    size_t accumulator = 0;
    for (const auto& submatrix : matrix.blocks) {
        accumulator += submatrix.raw_data.size() * submatrix.properties.degeneracy;
    }
    return accumulator;
}

size_t size_of_spectrum_without_degeneracy(const Spectrum& spectrum) {
    size_t accumulator = 0;
    for (const auto& subspectrum : spectrum.blocks) {
        accumulator += subspectrum.raw_data.size();
    }
    return accumulator;
}

size_t size_of_spectrum_with_degeneracy(const Spectrum& spectrum) {
    size_t accumulator = 0;
    for (const auto& subspectrum : spectrum.blocks) {
        accumulator += subspectrum.raw_data.size() * subspectrum.properties.degeneracy;
    }
    return accumulator;
}

void expect_spectrum_equivalence(const Spectrum& first, const Spectrum& second) {
    for (size_t i = 0; i < first.blocks.size(); ++i) {
        EXPECT_EQ(first.blocks[i].raw_data, second.blocks[i].raw_data);
    }
}

void EXPECT_SIZE_CONSISTENCE_OF_MATRICES(runner::Runner& runner) {
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_matrix_with_degeneracy(runner.getMatrix(common::QuantityEnum::Energy)));
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_matrix_without_degeneracy(runner.getMatrix(common::QuantityEnum::Energy)));
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_matrix_with_degeneracy(runner.getMatrix(common::QuantityEnum::S_total_squared)));
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_matrix_without_degeneracy(runner.getMatrix(common::QuantityEnum::S_total_squared)));
}

void EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner::Runner& runner) {
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_spectrum_with_degeneracy(runner.getSpectrum(common::QuantityEnum::Energy)));
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_spectrum_without_degeneracy(runner.getSpectrum(common::QuantityEnum::Energy)));
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_spectrum_with_degeneracy(
            runner.getSpectrum(common::QuantityEnum::S_total_squared)));
    EXPECT_EQ(
        runner.getIndexConverter().get_total_space_size(),
        size_of_spectrum_without_degeneracy(
            runner.getSpectrum(common::QuantityEnum::S_total_squared)));
}

TEST(matrix_and_spectrum_bulders, size_consistence_22_333_4444_23456) {
    std::vector<std::vector<int>> vector_of_mults =
        {{2, 2}, {3, 3, 3}, {4, 4, 4, 4}, {2, 3, 4, 5, 6}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);

        double J_value = 10;
        auto J = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols().assignSymbolToIsotropicExchange(J, 0, 1);
        model.InitializeSSquared();
        //
        {
            runner::Runner runner(model);
            runner.BuildMatrices();
            EXPECT_SIZE_CONSISTENCE_OF_MATRICES(runner);

            runner.BuildSpectra();
            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner);
        }
        // TZ_SORT
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.TzSort();
            runner::Runner runner(model, optimizationList);

            runner.BuildMatrices();
            EXPECT_SIZE_CONSISTENCE_OF_MATRICES(runner);

            runner.BuildSpectra();
            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner);
        }
    }
}

TEST(matrix_and_spectrum_bulders, size_consistence_2222_3333_4444) {
    std::vector<std::vector<int>> vector_of_mults = {{2, 2, 2, 2}, {3, 3, 3, 3}, {4, 4, 4, 4}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);
        double J_value = 10;
        auto J = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols()
            .assignSymbolToIsotropicExchange(J, 0, 1)
            .assignSymbolToIsotropicExchange(J, 1, 2)
            .assignSymbolToIsotropicExchange(J, 2, 3)
            .assignSymbolToIsotropicExchange(J, 3, 0);

        model.InitializeSSquared();
        // S2_S2_symmetrize
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.Symmetrize(group::Group::S2, {{1, 0, 3, 2}})
                .Symmetrize(group::Group::S2, {{3, 2, 1, 0}});

            runner::Runner runner(model, optimizationList);

            runner.BuildMatrices();
            EXPECT_SIZE_CONSISTENCE_OF_MATRICES(runner);

            runner.BuildSpectra();
            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner);
        }
        // TZ_SORT + S2_S2_symmetrize
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.TzSort()
                .Symmetrize(group::Group::S2, {{1, 0, 3, 2}})
                .Symmetrize(group::Group::S2, {{3, 2, 1, 0}});

            runner::Runner runner(model, optimizationList);

            runner.BuildMatrices();
            EXPECT_SIZE_CONSISTENCE_OF_MATRICES(runner);

            runner.BuildSpectra();
            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner);
        }
    }
}

TEST(matrix_and_spectrum_bulders, size_consistence_222_333_444) {
    std::vector<std::vector<int>> vector_of_mults = {{2, 2, 2}, {3, 3, 3}, {4, 4, 4}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);
        double J_value = 10;
        auto J = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols()
            .assignSymbolToIsotropicExchange(J, 0, 1)
            .assignSymbolToIsotropicExchange(J, 1, 2)
            .assignSymbolToIsotropicExchange(J, 2, 0);
        model.InitializeSSquared();
        // S3_SYMMETRIZE
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});

            runner::Runner runner(model, optimizationList);

            runner.BuildMatrices();
            EXPECT_SIZE_CONSISTENCE_OF_MATRICES(runner);
            runner.BuildSpectra();
            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner);
        }
        // TZ_SORT + S3_SYMMETRIZE
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.TzSort().Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});
            runner::Runner runner(model, optimizationList);

            runner.BuildMatrices();
            EXPECT_SIZE_CONSISTENCE_OF_MATRICES(runner);
            runner.BuildSpectra();
            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner);
        }
        // S3_SYMMETRIZE + NON_ABELIAN_SIMPLIFY
        //        {
        //            runner::Runner runner(model);
        //
        //            runner.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});
        //            runner.NonAbelianSimplify();
        //
        //            runner.BuildMatrices();
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_with_degeneracy(runner.getMatrix(common::QuantityEnum::Energy)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_without_degeneracy(runner.getMatrix(common::QuantityEnum::Energy)));
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_with_degeneracy(
        //                    runner.getMatrix(common::QuantityEnum::S_total_squared)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_without_degeneracy(
        //                    runner.getMatrix(common::QuantityEnum::S_total_squared)));
        //            runner.BuildSpectra();
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_with_degeneracy(runner.getSpectrum(common::QuantityEnum::Energy)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_without_degeneracy(
        //                    runner.getSpectrum(common::QuantityEnum::Energy)));
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_with_degeneracy(
        //                    runner.getSpectrum(common::QuantityEnum::S_total_squared)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_without_degeneracy(
        //                    runner.getSpectrum(common::QuantityEnum::S_total_squared)));
        //        }
        // TZ_SORT + S3_SYMMETRIZE + NON_ABELIAN_SIMPLIFY
        //        {
        //            runner::Runner runner(model);
        //
        //            runner.TzSort();
        //            runner.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});
        //            runner.NonAbelianSimplify();
        //
        //            runner.BuildMatrices();
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_with_degeneracy(runner.getMatrix(common::QuantityEnum::Energy)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_without_degeneracy(runner.getMatrix(common::QuantityEnum::Energy)));
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_with_degeneracy(
        //                    runner.getMatrix(common::QuantityEnum::S_total_squared)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_matrix_without_degeneracy(
        //                    runner.getMatrix(common::QuantityEnum::S_total_squared)));
        //            runner.BuildSpectra();
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_with_degeneracy(runner.getSpectrum(common::QuantityEnum::Energy)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_without_degeneracy(
        //                    runner.getSpectrum(common::QuantityEnum::Energy)));
        //            EXPECT_EQ(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_with_degeneracy(
        //                    runner.getSpectrum(common::QuantityEnum::S_total_squared)));
        //            EXPECT_NE(
        //                runner.getIndexConverter().get_total_space_size(),
        //                size_of_spectrum_without_degeneracy(
        //                    runner.getSpectrum(common::QuantityEnum::S_total_squared)));
        //        }
    }
}

//TEST(matrix_and_spectrum_bulders, size_consistence_222222222_tzsort_S3_S3_symmetrize_nonabeliansimplify) {
//    std::vector<int> mults = {2, 2, 2,
//                              2, 2, 2,
//                              2, 2, 2};
//
//    runner::Runner runner(mults);
//
//    runner.TzSort();
//    runner.Symmetrize(Group::S3, {{1, 2, 0, 4, 5, 3, 7, 8, 6}, {0, 2, 1, 3, 5, 4, 6, 8, 7}});
//    runner.NonAbelianSimplify();
//    runner.Symmetrize(Group::S3, {{3, 4, 5, 6, 7, 8, 0, 1, 2}, {0, 1, 2, 6, 7, 8, 3, 4, 5}});
//    runner.NonAbelianSimplify();
//
//    arma::dmat js(mults.size(), mults.size());
//    for (size_t i = 0; i < mults.size(); ++i) {
//        for (size_t j = 0; j < mults.size(); ++j) {
//            js(i, j) = NAN;
//        }
//    }
//
//    runner.AddIsotropicExchange(js);
//
//    runner.BuildMatrices();
//
//    EXPECT_EQ(runner.getIndexConverter().get_total_space_size(), size_of_matrix_with_degeneracy(runner.getMatrix(QuantityEnum::Energy)));
//    EXPECT_NE(runner.getIndexConverter().get_total_space_size(), size_of_matrix_without_degeneracy(runner.getMatrix(QuantityEnum::Energy)));
//}

TEST(spectrum_builder_without_matrix, size_consistence_and_spectra_equivalence_22_333_4444_23456) {
    std::vector<std::vector<int>> vector_of_mults =
        {{2, 2}, {3, 3, 3}, {4, 4, 4, 4}, {2, 3, 4, 5, 6}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);
        double J_value = 10;
        auto J = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols().assignSymbolToIsotropicExchange(J, 0, 1);
        model.InitializeSSquared();
        //
        {
            runner::Runner runner_without_matrices(model);
            runner::Runner runner_using_matrices(model);

            runner_using_matrices.BuildMatrices();

            runner_without_matrices.BuildSpectra();
            runner_using_matrices.BuildSpectra();

            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner_without_matrices);

            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }
        // TZ_SORT
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.TzSort();

            runner::Runner runner_without_matrices(model, optimizationList);
            runner::Runner runner_using_matrices(model, optimizationList);

            runner_using_matrices.BuildMatrices();

            runner_without_matrices.BuildSpectra();
            runner_using_matrices.BuildSpectra();

            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner_without_matrices);

            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }
    }
}

TEST(spectrum_builder_without_matrix, size_consistence_and_spectra_equivalence_2222_3333_4444) {
    std::vector<std::vector<int>> vector_of_mults = {{2, 2, 2, 2}, {3, 3, 3, 3}, {4, 4, 4, 4}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);
        double J_value = 10;
        auto J = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols()
            .assignSymbolToIsotropicExchange(J, 0, 1)
            .assignSymbolToIsotropicExchange(J, 1, 2)
            .assignSymbolToIsotropicExchange(J, 2, 3)
            .assignSymbolToIsotropicExchange(J, 3, 0);
        model.InitializeSSquared();

        // S2_S2_SYMMETRIZE
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.Symmetrize(group::Group::S2, {{1, 0, 3, 2}})
                .Symmetrize(group::Group::S2, {{3, 2, 1, 0}});

            runner::Runner runner_without_matrices(model, optimizationList);
            runner::Runner runner_using_matrices(model, optimizationList);

            runner_using_matrices.BuildMatrices();

            runner_without_matrices.BuildSpectra();
            runner_using_matrices.BuildSpectra();

            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner_without_matrices);

            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }
        // TZ_SORT + S2_S2_SYMMETRIZE
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.TzSort()
                .Symmetrize(group::Group::S2, {{1, 0, 3, 2}})
                .Symmetrize(group::Group::S2, {{3, 2, 1, 0}});

            runner::Runner runner_without_matrices(model, optimizationList);
            runner::Runner runner_using_matrices(model, optimizationList);

            runner_using_matrices.BuildMatrices();

            runner_without_matrices.BuildSpectra();
            runner_using_matrices.BuildSpectra();

            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner_without_matrices);

            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }
    }
}

TEST(
    spectrum_builder_without_matrix,
    size_consistence_and_spectra_equivalence_222_333_444_S3_symmetrize) {
    std::vector<std::vector<int>> vector_of_mults = {{2, 2, 2}, {3, 3, 3}, {4, 4, 4}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);
        double J_value = 10;
        auto J = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols()
            .assignSymbolToIsotropicExchange(J, 0, 1)
            .assignSymbolToIsotropicExchange(J, 1, 2)
            .assignSymbolToIsotropicExchange(J, 2, 0);
        model.InitializeSSquared();
        // S3_SYMMETRIZE
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});

            runner::Runner runner_without_matrices(model, optimizationList);
            runner::Runner runner_using_matrices(model, optimizationList);

            runner_using_matrices.BuildMatrices();

            runner_without_matrices.BuildSpectra();
            runner_using_matrices.BuildSpectra();

            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner_without_matrices);

            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }
        // TZ_SORT + S3_SYMMETRIZE
        {
            common::physical_optimization::OptimizationList optimizationList;
            optimizationList.TzSort().Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});

            runner::Runner runner_without_matrices(model, optimizationList);
            runner::Runner runner_using_matrices(model, optimizationList);

            runner_using_matrices.BuildMatrices();

            runner_without_matrices.BuildSpectra();
            runner_using_matrices.BuildSpectra();

            EXPECT_SIZE_CONSISTENCE_OF_SPECTRA(runner_without_matrices);

            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            expect_spectrum_equivalence(
                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }
        // S3_SYMMETRIZE + NON_ABELIAN_SIMPLIFY
        {
            //            runner::Runner runner_without_matrices(model);
            //            runner::Runner runner_using_matrices(model);
            //
            //            runner_without_matrices.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});
            //            runner_using_matrices.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});
            //            runner_without_matrices.NonAbelianSimplify();
            //            runner_using_matrices.NonAbelianSimplify();
            //
            //            runner_using_matrices.BuildMatrices();
            //
            //            runner_without_matrices.BuildSpectra();
            //            runner_using_matrices.BuildSpectra();
            //
            //            EXPECT_EQ(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_with_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::Energy)));
            //            EXPECT_NE(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_without_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::Energy)));
            //            EXPECT_EQ(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_with_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared)));
            //            EXPECT_NE(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_without_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared)));
            //
            //            expect_spectrum_equivalence(
            //                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
            //                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            //            expect_spectrum_equivalence(
            //                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
            //                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }  // TZ_SORT + S3_SYMMETRIZE + NON_ABELIAN_SIMPLIFY
        {
            //            runner::Runner runner_without_matrices(model);
            //            runner::Runner runner_using_matrices(model);
            //
            //            runner_without_matrices.TzSort();
            //            runner_using_matrices.TzSort();
            //            runner_without_matrices.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});
            //            runner_using_matrices.Symmetrize(group::Group::S3, {{1, 2, 0}, {0, 2, 1}});
            //            runner_without_matrices.NonAbelianSimplify();
            //            runner_using_matrices.NonAbelianSimplify();
            //
            //            runner_using_matrices.BuildMatrices();
            //
            //            runner_without_matrices.BuildSpectra();
            //            runner_using_matrices.BuildSpectra();
            //
            //            EXPECT_EQ(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_with_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::Energy)));
            //            EXPECT_NE(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_without_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::Energy)));
            //            EXPECT_EQ(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_with_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared)));
            //            EXPECT_NE(
            //                runner_without_matrices.getIndexConverter().get_total_space_size(),
            //                size_of_spectrum_without_degeneracy(
            //                    runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared)));
            //
            //            expect_spectrum_equivalence(
            //                runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
            //                runner_using_matrices.getSpectrum(common::QuantityEnum::Energy));
            //            expect_spectrum_equivalence(
            //                runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
            //                runner_using_matrices.getSpectrum(common::QuantityEnum::S_total_squared));
        }
    }
}

TEST(spectrum_builder_apply_to_entity, spectra_equivalence_22_333_4444_23456) {
    std::vector<std::vector<int>> vector_of_mults =
        {{2, 2}, {3, 3, 3}, {4, 4, 4, 4}, {2, 3, 4, 5, 6}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);
        double J_value = 10;
        auto J = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols().assignSymbolToIsotropicExchange(J, 0, 1);
        model.InitializeSSquared();

        runner::Runner runner_without_matrices(model);
        runner::Runner runner_to_manually_call_apply_to_entity(model);

        runner_to_manually_call_apply_to_entity.BuildMatrices();

        runner_without_matrices.BuildSpectra();

        std::vector<DenseMatrix> unitary_transformation_matrices(
            runner_to_manually_call_apply_to_entity.getMatrix(common::QuantityEnum::Energy)
                .blocks.size());
        Spectrum manually_energy_spectrum = Spectrum::energy(
            runner_to_manually_call_apply_to_entity.getMatrix(common::QuantityEnum::Energy),
            unitary_transformation_matrices);
        Spectrum manually_s_squared_spectrum = Spectrum::non_energy(
            runner_to_manually_call_apply_to_entity.getMatrix(
                common::QuantityEnum::S_total_squared),
            unitary_transformation_matrices);

        expect_spectrum_equivalence(
            runner_without_matrices.getSpectrum(common::QuantityEnum::Energy),
            manually_energy_spectrum);
        expect_spectrum_equivalence(
            runner_without_matrices.getSpectrum(common::QuantityEnum::S_total_squared),
            manually_s_squared_spectrum);
    }
}

TEST(
    spectrum_builder_apply_to_entity,
    throw_size_inconsistent_non_energy_matrix_22_333_4444_23456) {
    std::vector<std::vector<int>> vector_of_mults =
        {{2, 2}, {3, 3, 3}, {4, 4, 4, 4}, {2, 3, 4, 5, 6}};

    for (const auto& mults : vector_of_mults) {
        model::Model model(mults);
        double J_value = 10;
        auto J_tz_sorted = model.getSymbols().addSymbol("J", J_value);
        model.getSymbols().assignSymbolToIsotropicExchange(J_tz_sorted, 0, 1);
        model.InitializeSSquared();

        common::physical_optimization::OptimizationList optimizationList;
        optimizationList.TzSort();

        runner::Runner runner_tz_sorted(model, optimizationList);
        runner::Runner runner_not_tz_sorted_to_manually_call_apply_to_entity(model);

        runner_tz_sorted.BuildMatrices();
        runner_not_tz_sorted_to_manually_call_apply_to_entity.BuildMatrices();

        runner_tz_sorted.BuildSpectra();

        std::vector<DenseMatrix> unitary_transformation_matrices(
            runner_not_tz_sorted_to_manually_call_apply_to_entity
                .getMatrix(common::QuantityEnum::Energy)
                .blocks.size());
        Spectrum manually_energy_spectrum = Spectrum::energy(
            runner_not_tz_sorted_to_manually_call_apply_to_entity.getMatrix(
                common::QuantityEnum::Energy),
            unitary_transformation_matrices);
        EXPECT_THROW(
            Spectrum::non_energy(
                runner_tz_sorted.getMatrix(common::QuantityEnum::S_total_squared),
                unitary_transformation_matrices),
            std::length_error);
    }
}