/******************************************************************************
Copyright (c) 2021 Dmitriy Korchemkin

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
******************************************************************************/

#ifndef POLYNOMIALS_ROOTS_HPP
#define POLYNOMIALS_ROOTS_HPP

#include <Eigen/Dense>
#include <iosfwd>

#include "polynomials/assert.hpp"
#include "polynomials/types.hpp"

#ifndef CERES_PUBLIC_JET_H
namespace ceres {
template <typename Scalar, int N> struct Jet;
}
#endif

namespace polynomials {

template <typename Scalar> struct RealRootFilter {
  std::optional<Scalar> operator()(const std::complex<Scalar> &c,
                                   const Scalar &tolerance) const {
    using std::abs;
    const auto re = c.real();
    const auto im = c.imag();
    const auto abs_re = abs(re);
    const auto abs_im = abs(im);
    if (abs_im < abs_re * tolerance)
      return re;
    return std::nullopt;
  }

  std::optional<Scalar> operator()(const std::complex<Scalar> &c) const {
    const auto re = c.real();
    const auto im = c.imag();
    if (im == Scalar(0.))
      return re;
    return std::nullopt;
  }
};

template <typename Scalar> struct PositiveRealRootFilter {
  std::optional<Scalar> operator()(const std::complex<Scalar> &c) const {
    auto real = rrf(c);
    if (!real)
      return std::nullopt;
    if (*real >= Scalar(0.))
      return real;
    return std::nullopt;
  }

  std::optional<Scalar> operator()(const std::complex<Scalar> &c,
                                   const Scalar &tolerance) const {
    auto real = rrf(c, tolerance);
    if (!real)
      return std::nullopt;
    if (*real >= Scalar(0.))
      return real;
    return std::nullopt;
  }

  RealRootFilter<Scalar> rrf;
};

template <typename Polynomial,
          bool fixed_degree = Polynomial::DegreeAtCompileTime != Dynamic>
struct QuotientRingMulXImpl;

template <typename Polynomial>
struct QuotientRingMulX : public QuotientRingMulXImpl<Polynomial> {
  using Base = QuotientRingMulXImpl<Polynomial>;

  template <typename... Args> QuotientRingMulX(Args... args) : Base(args...) {}
};

template <typename Polynomial> struct QuotientRingMulXImpl<Polynomial, true> {
  static constexpr Index DegreeAtCompileTime = Polynomial::DegreeAtCompileTime;
  static constexpr Index MaxDegreeAtCompileTime =
      Polynomial::MaxDegreeAtCompileTime;
  using Scalar = typename Polynomial::Scalar;

  using CompanionMatrix =
      Eigen::Matrix<Scalar, DegreeAtCompileTime, DegreeAtCompileTime>;
  using ComplexRoots = typename CompanionMatrix::EigenvaluesReturnType;
  using RealRoots =
      Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, DegreeAtCompileTime, 1>;

  QuotientRingMulXImpl(const Polynomial &p) {
    const Scalar lead_term = p[DegreeAtCompileTime];
    matrix
        .template bottomLeftCorner<DegreeAtCompileTime - 1,
                                   DegreeAtCompileTime - 1>()
        .setIdentity();
    matrix.col(DegreeAtCompileTime - 1) =
        -p.coeffs().template head<DegreeAtCompileTime>() *
        (Scalar(1.) / lead_term);
    matrix.row(0).template head<DegreeAtCompileTime - 1>().setZero();
  }

  operator CompanionMatrix() const { return matrix; }

  ComplexRoots complex_roots() const { return matrix.eigenvalues(); }

  template <typename Op, typename... Args>
  RealRoots filter_roots(Args... args) const {
    Index count_valid = 0;
    Scalar rr[DegreeAtCompileTime];
    const auto roots = complex_roots();
    const Op filter;
    for (Index i = 0; i < DegreeAtCompileTime; ++i) {
      auto real = filter(roots[i], args...);
      if (!real)
        continue;
      rr[count_valid++] = *real;
    }
    return Eigen::Map<const RealRoots>(rr, count_valid, 1);
  }

  template <typename... Args> RealRoots real_roots(Args... args) const {
    return filter_roots<RealRootFilter<Scalar>, Args...>(args...);
  }

  template <typename... Args>
  RealRoots positive_real_roots(Args... args) const {
    return filter_roots<PositiveRealRootFilter<Scalar>, Args...>(args...);
  }

private:
  CompanionMatrix matrix;
};

template <typename Polynomial> struct QuotientRingMulXImpl<Polynomial, false> {
  static constexpr Index DegreeAtCompileTime = Polynomial::DegreeAtCompileTime;
  static constexpr Index MaxDegreeAtCompileTime =
      Polynomial::MaxDegreeAtCompileTime;
  using Scalar = typename Polynomial::Scalar;

  using CompanionMatrix =
      Eigen::Matrix<Scalar, DegreeAtCompileTime, DegreeAtCompileTime, 0,
                    MaxDegreeAtCompileTime, MaxDegreeAtCompileTime>;
  using ComplexRoots = typename CompanionMatrix::EigenvaluesReturnType;
  using RealRoots =
      Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, MaxDegreeAtCompileTime, 1>;

  QuotientRingMulXImpl(const Polynomial &p) {
    const Index degree = p.degree();
    matrix.resize(degree, degree);
    const Scalar lead_term = p[degree];
    matrix.bottomLeftCorner(degree - 1, degree - 1).setIdentity();
    matrix.col(DegreeAtCompileTime - 1) =
        -p.coeffs().head(degree) * (Scalar(1.) / lead_term);
    matrix.row(0).head(degree - 1).setZero();
  }

  operator CompanionMatrix() const { return matrix; }

  ComplexRoots complex_roots() const { return matrix.eigenvalues(); }

  RealRoots real_roots() const {
    Index count_real = 0;
    const Index degree = dim();
    Eigen::Matrix<Scalar, MaxDegreeAtCompileTime, 1> rr(degree, 1);

    const auto roots = complex_roots();

    for (Index i = 0; i < degree; ++i) {
      auto im = roots[i].imag();
      if (im != Scalar(0.))
        continue;
      auto re = roots[i].real();
      rr[count_real++] = re;
    }
    return Eigen::Map<const RealRoots>(rr, count_real, 1);
  }

  RealRoots real_roots(const Scalar &tolerance) const {
    Index count_real = 0;
    const Index degree = dim();
    Eigen::Matrix<Scalar, MaxDegreeAtCompileTime, 1> rr(degree, 1);

    const auto roots = complex_roots();

    for (Index i = 0; i < degree; ++i) {
      auto re = roots[i].real();
      auto are = std::abs(re);
      auto aim = std::abs(roots[i].imag);
      if (!(aim < tolerance * are))
        continue;
      rr[count_real++] = re;
    }
    return Eigen::Map<const RealRoots>(rr, count_real, 1);
  }

  Index dim() const { return matrix.rows(); }

private:
  CompanionMatrix matrix;
};

template <typename Poly, template <typename> typename Algo, typename Scalar>
struct RootFinder {
  using Algorithm = Algo<Poly>;
  using ComplexRoots = typename Algorithm::ComplexRoots;
  using RealRoots = typename Algorithm::RealRoots;

  template <typename... Args>
  static RealRoots real_roots(const Poly &p, Args... args) {
    return Algorithm(p).real_roots(args...);
  }

  template <typename... Args>
  static RealRoots positive_real_roots(const Poly &p, Args... args) {
    return Algorithm(p).positive_real_roots(args...);
  }

  template <typename... Args>
  static ComplexRoots complex_roots(const Poly &p, Args... args) {
    return Algorithm(p).complex_roots(args...);
  }
};

template <typename Poly, template <typename> typename Algo, typename Scalar,
          int N>
struct RootFinder<Poly, Algo, ceres::Jet<Scalar, N>> {
  using ScalarPoly = DensePoly<Scalar, Poly::DegreeAtCompileTime,
                               Poly::MaxDegreeAtCompileTime>;
  using ScalarRootFinder = RootFinder<ScalarPoly, Algo>;
  using ScalarAlgorithm = Algo<ScalarPoly>;
  using ScalarComplexRoots = typename ScalarAlgorithm::ComplexRoots;
  using ScalarRealRoots = typename ScalarAlgorithm::RealRoots;
  using Jet = ceres::Jet<Scalar, N>;
  using ComplexJet = ceres::Jet<typename ScalarComplexRoots::Scalar, N>;

  using JetComplexRoots = Eigen::Matrix<ComplexJet, Poly::DegreeAtCompileTime,
                                        1, 0, Poly::MaxDegreeAtCompileTime, 1>;
  using JetRealRoots =
      Eigen::Matrix<Jet, Eigen::Dynamic, 1, 0, Poly::MaxDegreeAtCompileTime, 1>;

  static ScalarPoly cast(const Poly &p) {
    ScalarPoly sp(p.degree());
    const Index num_coeffs = p.degree() + 1;
    for (Index i = 0; i < num_coeffs; ++i)
      sp.coeffs()[i] = p.coeffs()[i].a;
    return sp;
  }

  template <typename T>
  static ceres::Jet<T, N>
  process_root(const Poly &poly, const ScalarPoly &scalar_poly, const T &root) {
    ceres::Jet<T, N> res;
    res.a = root;
    res.v.setZero();
    const T denom(-scalar_poly.df(root));
    const auto &coeffs = poly.coeffs();
    const Index num_coeffs = coeffs.size();
    T root_pow = T(1.);
    for (Index i = 0; i < num_coeffs; ++i) {
      // dx / da_k = -x^k / f'
      // dx / dp_i = \sum_k dx / da_k da_k / dp_i
      // dx / dp = (\sum_k -x^k [da_k / dp]) / f'

      res.v += root_pow * coeffs[i].v;
      root_pow *= root;
    }
    res.v /= denom;
    return res;
  }

  template <typename... Args>
  static JetComplexRoots complex_roots(const Poly &p, Args... args) {
    const ScalarPoly scalar = cast(p);
    const ScalarComplexRoots scalar_roots =
        ScalarRootFinder::complex_roots(scalar, args...);
    const Index num_roots = scalar_roots.size();
    JetComplexRoots jet(num_roots);
    for (Index i = 0; i < num_roots; ++i)
      jet[i] = process_root(p, scalar, scalar_roots[i]);
    return jet;
  }

  template <typename... Args>
  static JetRealRoots real_roots(const Poly &p, Args... args) {
    const ScalarPoly scalar = cast(p);
    const ScalarRealRoots scalar_roots =
        ScalarRootFinder::real_roots(scalar, args...);
    const Index num_real_roots = scalar_roots.size();
    JetRealRoots jet(num_real_roots);
    for (Index i = 0; i < num_real_roots; ++i)
      jet[i] = process_root(p, scalar, scalar_roots[i]);
    return jet;
  }

  template <typename... Args>
  static JetRealRoots positive_real_roots(const Poly &p, Args... args) {
    const ScalarPoly scalar = cast(p);
    const ScalarRealRoots scalar_roots =
        ScalarRootFinder::positive_real_roots(scalar, args...);
    const Index num_real_roots = scalar_roots.size();
    JetRealRoots jet(num_real_roots);
    for (Index i = 0; i < num_real_roots; ++i)
      jet[i] = process_root(p, scalar, scalar_roots[i]);
    return jet;
  }
};

} // namespace polynomials

#endif


/*
Copyright 2014 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Test product.hpp.
*/

#define BOOST_TEST_MODULE test_math_product
#include "utility/test/boost_unit_test.hpp"

#include "math/product.hpp"

#include <string>
#include <vector>

#include <boost/mpl/assert.hpp>

#include "math/arithmetic_magma.hpp"
#include "math/cost.hpp"
#include "math/sequence.hpp"
#include "math/check/check_magma.hpp"

BOOST_AUTO_TEST_SUITE (test_suite_product)

template <class Inverses> void test_product_spot_check_without_inverses() {
    typedef math::product <math::over <float, math::sequence <char>>, Inverses>
        product;

    // Construction.
    {
        product p (1.5f, math::sequence <char> (std::string ("!")));
        BOOST_CHECK_EQUAL (range::first (p.components()), 1.5f);
        BOOST_CHECK_EQUAL (
            range::first (range::second (p.components()).symbols()), '!');

        // Use make_product.
        auto p2 = math::make_product <Inverses> (
            2.5f, math::sequence <char> (std::string ("?")));

        static_assert (std::is_same <decltype (p2), product>::value, "");
        BOOST_CHECK_EQUAL (range::first (p2.components()), 2.5f);
        BOOST_CHECK_EQUAL (
            range::first (range::second (p2.components()).symbols()), '?');

        // Use make_product_over.
        range::tuple <float, math::sequence <char>> components (
            4.5f, math::sequence <char> (std::string ("~")));
        auto p3 = math::make_product_over <Inverses> (components);

        static_assert (std::is_same <decltype (p3), product>::value, "");
        BOOST_CHECK_EQUAL (range::first (p3.components()), 4.5f);
        BOOST_CHECK_EQUAL (
            range::first (range::second (p3.components()).symbols()), '~');
    }

    // is_member.
    {
        product not_a_member (math::non_member <float>(),
            math::sequence <char> (std::string ("a")));
        BOOST_CHECK (!math::is_member (not_a_member));

        product member (0,
            math::sequence <char> (std::string ("a")));
        BOOST_CHECK (math::is_member (member));

        // Check that one non-member makes a non-member.
        math::product <math::over <float, float>> not_a_member_2 (
            math::non_member <float>(), 0);
        BOOST_CHECK (!math::is_member (not_a_member_2));

        math::product <math::over <float, float>> not_a_member_3 (
            math::non_member <float>(), math::non_member <float>());
        BOOST_CHECK (!math::is_member (not_a_member_3));
    }

    // equal.
    {
        product a1 (1, math::sequence <char> (std::string ("a")));
        product a1_2 (1, math::sequence <char> (std::string ("a")));
        product a1_3 (1, math::single_sequence <char> ('a'));
        product a1_approximate (1.000001, math::single_sequence <char> ('a'));
        product b1 (1, math::sequence <char> (std::string ("b")));
        product a2 (2, math::sequence <char> (std::string ("a")));
        product nothing2_1 (2, math::sequence <char> (std::string ("")));
        product nothing2_2 (2, math::empty_sequence <char>());
        product annihilator3_1 (3,
            math::sequence <char> (math::sequence_annihilator <char>()));
        product annihilator3_2 (3, math::sequence_annihilator <char>());

        BOOST_CHECK (math::equal (a1, a1));
        BOOST_CHECK (math::approximately_equal (a1, a1));

        BOOST_CHECK (math::equal (a1, a1_2));
        BOOST_CHECK (math::approximately_equal (a1, a1_2));

        BOOST_CHECK (math::equal (a1, a1_3));
        BOOST_CHECK (math::approximately_equal (a1, a1_3));

        BOOST_CHECK (!math::equal (a1, a1_approximate));
        BOOST_CHECK (math::approximately_equal (a1, a1_approximate));

        BOOST_CHECK (!math::equal (a1, b1));
        BOOST_CHECK (!math::approximately_equal (a1, b1));

        BOOST_CHECK (!math::equal (a1, a2));
        BOOST_CHECK (!math::approximately_equal (a1, a2));

        BOOST_CHECK (!math::equal (a1, nothing2_1));
        BOOST_CHECK (!math::approximately_equal (a1, nothing2_1));

        BOOST_CHECK (!math::equal (a1, nothing2_2));
        BOOST_CHECK (!math::approximately_equal (a1, nothing2_2));

        BOOST_CHECK (math::equal (nothing2_1, nothing2_2));
        BOOST_CHECK (math::approximately_equal (nothing2_1, nothing2_2));

        BOOST_CHECK (math::equal (annihilator3_1, annihilator3_2));
        BOOST_CHECK (math::approximately_equal (
            annihilator3_1, annihilator3_2));
        BOOST_CHECK (!math::equal (a1, annihilator3_2));
        BOOST_CHECK (!math::approximately_equal (a1, annihilator3_2));
    }

    // Test identity <plus>.
    {
        auto zero = math::zero <product>();
        BOOST_CHECK_EQUAL (range::first (zero.components()), 0);
        BOOST_CHECK_EQUAL (range::at_c <1> (zero.components()),
            math::sequence_annihilator <char>());
    }
    {
        // Implicitly convert to "product".
        product zero = math::zero <product>();
        BOOST_CHECK_EQUAL (range::first (zero.components()), 0);
        BOOST_CHECK_EQUAL (range::at_c <1> (zero.components()),
            math::sequence_annihilator <char>());
    }

    // Test identity <times>.
    {
        auto one = math::one <product>();
        BOOST_CHECK_EQUAL (range::first (one.components()), 1);
        BOOST_CHECK_EQUAL (range::at_c <1> (one.components()),
            math::empty_sequence <char>());
    }
    {
        product one = math::one <product>();
        BOOST_CHECK_EQUAL (range::first (one.components()), 1);
        BOOST_CHECK_EQUAL (range::at_c <1> (one.components()),
            math::empty_sequence <char>());
    }

    // Test annihilator.
    {
        auto zero = math::annihilator <product, math::callable::times>();
        BOOST_CHECK_EQUAL (range::first (zero.components()), 0);
        BOOST_CHECK_EQUAL (range::at_c <1> (zero.components()),
            math::sequence_annihilator <char>());
    }
    {
        BOOST_MPL_ASSERT_NOT ((math::has <
            math::callable::annihilator <product, math::callable::plus>()>));
        // auto test = math::annihilator <product, math::callable::plus>();
    }

    {
        BOOST_MPL_ASSERT_NOT ((math::has <
            math::callable::invert <math::callable::plus> (product)>));
    }

    product a2 (2, math::sequence <char> (std::string ("a")));
    product b4 (4, math::sequence <char> (std::string ("b")));
    product ab8 = a2 * b4;
    BOOST_CHECK_EQUAL (range::first (ab8.components()), 8);
    BOOST_CHECK_EQUAL (range::at_c <1> (ab8.components()),
        math::sequence <char> (std::string ("ab")));
    BOOST_CHECK_EQUAL (ab8,
        product (8, math::sequence <char> (std::string ("ab"))));

    product a10 = ab8 + a2;
    BOOST_CHECK_EQUAL (range::first (a10.components()), 10);
    BOOST_CHECK_EQUAL (range::at_c <1> (a10.components()),
        math::sequence <char> (std::string ("a")));

    // compare.
    BOOST_MPL_ASSERT ((math::has <math::callable::compare (product, product)>));

    product a4 (4, math::sequence <char> (std::string ("a")));
    product b2 (2, math::sequence <char> (std::string ("b")));
    BOOST_CHECK (math::compare (a2, b4));
    BOOST_CHECK (!math::compare (a2, a2));
    BOOST_CHECK (!math::compare (b4, b4));
    BOOST_CHECK (math::compare (a2, b2));

    // Test reverse.
    {
        auto ab8_reverse = math::reverse <math::callable::times> (ab8);
        // Should be the same as ab8, except the sequence has become a right
        // sequence and is in reverse order.
        BOOST_MPL_ASSERT ((std::is_same <decltype (ab8_reverse),
            math::product <math::over <float,
                math::sequence <char, math::right>>, Inverses>>));

        BOOST_CHECK_EQUAL (range::first (ab8_reverse.components()), 8);
        BOOST_CHECK_EQUAL (range::at_c <1> (ab8_reverse.components()),
            (math::sequence <char, math::right> (std::string ("ba"))));
    }

    // Test properties.
    BOOST_MPL_ASSERT_NOT ((
        math::has <math::callable::minus <math::left> (product, product)>));
}

BOOST_AUTO_TEST_CASE (test_product_properties) {
    using math::product;
    using math::over;
    using math::sequence;
    using math::cost;

    using math::callable::plus;
    using math::callable::times;

    // approximate.
    {
        BOOST_MPL_ASSERT ((math::is::approximate <times (
            product <over <float, float>>,
            product <over <float, float>>)>));
        BOOST_MPL_ASSERT ((math::is::approximate <times (
            product <over <float, sequence <char>>>,
            product <over <float, sequence <char>>>)>));
        BOOST_MPL_ASSERT_NOT ((math::is::approximate <times (
            product <over <sequence <char>>>,
            product <over <sequence <char>>>)>));
    }
    // associative.
    {
        BOOST_MPL_ASSERT ((math::is::associative <times,
            product <over <float, sequence <char>>>>));
    }
    // commutative.
    {
        BOOST_MPL_ASSERT ((math::is::commutative <times,
            product <over <float, float>>>));
        BOOST_MPL_ASSERT ((math::is::commutative <plus,
            product <over <float, float>>>));

        BOOST_MPL_ASSERT_NOT ((math::is::commutative <times,
            product <over <float, sequence <char>>>>));
        BOOST_MPL_ASSERT ((math::is::commutative <plus,
            product <over <float, sequence <char>>>>));
    }
    // idempotent.
    {
        BOOST_MPL_ASSERT_NOT ((math::is::idempotent <times,
            product <over <float, float>>>));
        BOOST_MPL_ASSERT_NOT ((math::is::idempotent <plus,
            product <over <float, float>>>));

        BOOST_MPL_ASSERT_NOT ((math::is::idempotent <times,
            product <over <cost <float>, sequence <char>>>>));
        BOOST_MPL_ASSERT ((math::is::idempotent <plus,
            product <over <cost <float>, sequence <char>>>>));
    }
    // path_operation.
    {
        BOOST_MPL_ASSERT_NOT ((math::is::path_operation <times,
            product <over <float, float>>>));
        BOOST_MPL_ASSERT_NOT ((math::is::path_operation <plus,
            product <over <float, float>>>));

        BOOST_MPL_ASSERT_NOT ((math::is::path_operation <times,
            product <over <cost <float>, sequence <char>>>>));
        BOOST_MPL_ASSERT_NOT ((math::is::path_operation <plus,
            product <over <cost <float>, sequence <char>>>>));
        // Sanity
        BOOST_MPL_ASSERT ((math::is::path_operation <plus, cost <float>>));
        BOOST_MPL_ASSERT ((math::is::path_operation <plus,
            product <over <cost <float>, cost <float>>>>));
    }
}

BOOST_AUTO_TEST_CASE (test_product_spot_check) {
    test_product_spot_check_without_inverses <math::with_inverse<>>();
    {
        // Spot check for divide.
        test_product_spot_check_without_inverses <
            math::with_inverse <math::callable::times>>();

        typedef math::product <math::over <float, math::sequence <char>>,
            math::with_inverse <math::callable::times>> product;

        product a2 (2, math::sequence <char> (std::string ("a")));
        product b4 (4, math::sequence <char> (std::string ("b")));
        product ab8 = a2 * b4;

        product b4_again = math::divide <math::left> (ab8, a2);
        BOOST_CHECK_EQUAL (range::first (b4_again.components()), 4);
        BOOST_CHECK_EQUAL (range::at_c <1> (b4_again.components()),
            math::sequence <char> (std::string ("b")));
        BOOST_CHECK_EQUAL (b4_again, b4);
    }
    {
        // Spot check for plus.
        test_product_spot_check_without_inverses <
            math::with_inverse <math::callable::plus>>();

        // Spot check for minus.
        typedef math::product <math::over <float, int>,
            math::with_inverse <math::callable::plus>> product;

        product p1 (2, 3);
        product p2 (4, 8);

        product p3 = p1 * p2;
        BOOST_CHECK_EQUAL (p3, product (8, 24));

        product p4 = p1 + p2;
        BOOST_CHECK_EQUAL (p4, product (6., 11));

        product p1_again = p4 - p2;
        BOOST_CHECK_EQUAL (p1_again, p1);
        product p2_again = p4 - p1;
        BOOST_CHECK_EQUAL (p2_again, p2);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/serialization/export.hpp>
#include "MaxPooling2D.hpp"
#include "LayerModel.hpp"
#include "../../tools/Tools.hpp"

using namespace std;
using namespace snn;
using namespace internal;
using namespace tools;

BOOST_CLASS_EXPORT(MaxPooling2D)

MaxPooling2D::MaxPooling2D(LayerModel& model)
    : NoNeuronLayer(model)
{
    this->sizeOfFilterMatrix = model.sizeOfFilerMatrix;
    this->shapeOfInput = model.shapeOfInput;

    const int restX = shapeOfInput[0] % this->sizeOfFilterMatrix == 0 ? 0 : 1;
    const int restY = shapeOfInput[1] % this->sizeOfFilterMatrix == 0 ? 0 : 1;

    this->shapeOfOutput = {
        this->shapeOfInput[0] / this->sizeOfFilterMatrix + restX,
        this->shapeOfInput[1] / this->sizeOfFilterMatrix + restY,
        1
    };
}

inline
unique_ptr<BaseLayer> MaxPooling2D::clone(shared_ptr<NeuralNetworkOptimizer>) const
{
    return make_unique<MaxPooling2D>(*this);
}

std::vector<float> MaxPooling2D::output(const std::vector<float>& inputs, [[maybe_unused]] bool temporalReset)
{
    auto output = vector<float>(this->numberOfOutputs, numeric_limits<float>::lowest());
    for (int i = 0; i < (int)inputs.size(); ++i)
    {
        const int outputX = roughenX(i, this->shapeOfInput[0], this->shapeOfInput[1]) / this->sizeOfFilterMatrix;
        const int outputY = roughenY(i, this->shapeOfInput[0], this->shapeOfInput[1]) / this->sizeOfFilterMatrix;
        const int indexOutput = flatten(outputX, outputY, this->shapeOfOutput[0]);

        if (output[indexOutput] <= inputs[i])
            output[indexOutput] = inputs[i];
    }
    return output;
}

vector<float> MaxPooling2D::outputForTraining(const vector<float>& inputs, bool temporalReset)
{
    return this->output(inputs, temporalReset);
}

vector<float> MaxPooling2D::backOutput(std::vector<float>& inputErrors)
{
    std::vector<float> errors;
    errors.reserve(this->numberOfInputs);

    for (int z = 0; z < this->shapeOfInput[2]; ++z)
    {
        for (int y = 0; y < this->shapeOfInput[1]; ++y)
        {
            for (int x = 0; x < this->shapeOfInput[0]; ++x)
            {
                const int outputX = x / this->sizeOfFilterMatrix;
                const int outputY = y / this->sizeOfFilterMatrix;
                const int i = flatten(outputX, outputY, this->shapeOfOutput[0]);

                errors.push_back(inputErrors[i]);
            }
        }
    }
    return errors;
}

void MaxPooling2D::train([[maybe_unused]] std::vector<float>& inputErrors)
{
}

int MaxPooling2D::getNumberOfInputs() const
{
    return this->numberOfInputs;
}

std::vector<int> MaxPooling2D::getShapeOfInput() const
{
    return this->shapeOfInput;
}

vector<int> MaxPooling2D::getShapeOfOutput() const
{
    return this->shapeOfOutput;
}

int MaxPooling2D::isValid() const
{
    return 0;
}

inline
bool MaxPooling2D::operator==(const BaseLayer& layer) const
{
    try
    {
        const auto& l = dynamic_cast<const MaxPooling2D&>(layer);

        return typeid(*this).hash_code() == typeid(layer).hash_code()
            && this->sizeOfFilterMatrix == l.sizeOfFilterMatrix
            && this->shapeOfInput == l.shapeOfInput;
    }
    catch (std::bad_cast&)
    {
        return false;
    }
}

inline
bool MaxPooling2D::operator!=(const BaseLayer& layer) const
{
    return !(*this == layer);
}


#include <fstream>
#include <unordered_map>

#include <boost/format.hpp>

#include "local_push.h"
#include "file_serialization.h"
#include "util/log.h"
using boost::format;

double cal_rmax(double c, double epsilon) {
    return (1 - c) * epsilon;
}

double cal_rmax(DirectedG &g, double c, double epsilon, double delta) {
    // calculate r_max
    int m, n;
    m = num_edges(g);
    n = num_vertices(g);
    double d = double(m) / double(n);
    // cout << d << endl;
    double a = (1 - c) * pow(d, 2) * pow(epsilon, 2) / (c * log(2 / delta));
    // cout << pow(a,1.0/3.0) << endl;
    return pow(a, 1.0 / 3.0);
}

void LocalPush::push(NodePair &pab, double inc) {
    // the actually action of push
    n_push++;
    R[pab] += inc;
    // cout << pab.first << " " << pab.second << endl ;
    if (fabs(R[pab]) > r_max) {
        // Q.insert(pab);
        if (marker[pab] == false) {
            Q.push(pab);
            marker[pab] = true;
        }
    }
}

LocalPush::LocalPush(DirectedG &g, string gName_, double c_, double epsilon_, size_t n_) {
    // init data members
    /* init data memebrs */
    g_name = gName_;
    c = c_;
    // r_max = r_max_;
    epsilon = epsilon_;
    r_max = cal_rmax(c, epsilon);
    n = n_;
    string path = get_edge_list_path(gName_);
    n_push = 0;
    cpu_time = -1; // set the init value

    // check the local puah exits
    // string data_path =get_file_path_base() + ".P";
    // cout << "data path " << data_path << endl;
    // if(file_exists(data_path)){ // local push from start
    //     cout << "file exisis.." << endl;
    //     load();
    // }else{ // file exists
    //     /* init the P and R */
    //     cout << "file not exists, compute from scratch" << endl;
    //     for(int i=0;i<n;i++){
    //         NodePair np(i,i);
    //         R.insert({np,1});
    //         Q.push(np);
    //         marker[np] = true;
    //     }
    // }
}

Full_LocalPush::Full_LocalPush(DirectedG &g, string name, double c_, double r_max_, size_t n_) : LocalPush(g, name, c_,
                                                                                r_max_, n_) {
    string data_path = get_file_path_base() + ".P";
    cout << "data path " << data_path << endl;
    if (file_exists(data_path)) { // local push from start
        cout << "file exisis.." << endl;
        load();
    } else { // file exists
        /* init the P and R */
        P.add(n);
        R.add(n);
        marker.add(n);

        cout << "file not exists, compute from scratch" << endl;
        for (int i = 0; i < n; i++) {
            NodePair np(i, i);
            // R.insert({np,1});
            R[np] = 1;
            Q.push(np);
            marker[np] = true;
        }
    }
}

Reduced_LocalPush::Reduced_LocalPush(DirectedG &g, string name, double c_, double r_max_, size_t n_) : LocalPush(g,
                                                                                name,
                                                                                c_,
                                                                                r_max_,
                                                                                n_) {
    string data_path = get_file_path_base() + ".P";
    // cout << "data path " << data_path << endl;
    if (file_exists(data_path)) { // local push from start
        // cout << "file exisis.." << endl;
        load();
        // cout << g_name << " " << r_max << " " << c << " " << n << endl;
    } else { // file exists
        /* init the P and R */
        P.add(n);
        R.add(n);
        marker.add(n);

        cout << "file not exists, compute from scratch!!!" << endl;
        for (int i = 0; i < n; i++) {
            NodePair np(i, i);
            // R.insert({np,1});
            R[np] = 1;
            Q.push(np);
            marker[np] = true;
        }
    }
}

void Reduced_LocalPush::push_to_neighbors(DirectedG &g, NodePair &np, double current_residual) {
    // the push method using reduced linear system
    // out-neighbros of a,b
    DirectedG::out_edge_iterator
            outi_iter,
            outi_end,
            outj_iter,
            outj_end;
    bool is_singleton = np.first == np.second ? true : false;
    // /* only push to partial pairs*/
    size_t out_degree_i = out_degree(np.first, g);
    size_t out_degree_j = out_degree(np.second, g);

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
    /*the indicator whether the position is common neighbor */
    vector<bool> outs_i_common(out_degree_i, false);
    vector<bool> outs_j_common(out_degree_j, false);
    // cout << "------" << endl;

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    if (is_singleton) {
        /* starting push for singleton nodes*/
        for (; outi_iter != outi_end; outi_iter++) {
            tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
            auto out_a = target(*outi_iter, g); // out-neighbor
            for (; outj_iter != outj_end; outj_iter++) {
                auto out_b = target(*outj_iter, g);
                auto indegree_a = in_degree(out_a, g);
                auto indegree_b = in_degree(out_b, g);
                auto total_in = indegree_a * indegree_b;
                if (out_a < out_b) { // only push to partial pairs for a < b
                    NodePair pab(out_a, out_b); // the node-pair to be pushed to
                    double inc = c * current_residual / total_in;
                    push(pab, inc); // do the push action
                }
            }
        }
    } else {
        /* mark the common neighbors */
        size_t i, j;
        for (i = 0; i < out_degree_i; i++) {
            auto a = target(*(outi_iter + i), g);
            for (j = 0; j < out_degree_j; j++) {
                auto b = target(*(outj_iter + j), g);
                if (a == b) {
                    outs_i_common[i] = true;
                    outs_j_common[j] = true;
                    break;
                }
            }
        }
        /* starting push for non-singleton nodes*/
        auto i_begin_iter = outi_iter; // mark the begining iterator
        for (; outi_iter != outi_end; outi_iter++) {
            tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
            auto j_begin = outj_iter;
            bool is_i_common = outs_i_common[outi_iter - i_begin_iter];// indicator of whether i is a common neighbor
            for (; outj_iter != outj_end; outj_iter++) {
                bool is_j_common = outs_j_common[outj_iter - j_begin];
                auto out_a = target(*outi_iter, g); // out-neighbor
                auto out_b = target(*outj_iter, g);
                auto indegree_a = in_degree(out_a, g);
                auto indegree_b = in_degree(out_b, g);
                auto total_in = indegree_a * indegree_b;
                double inc = c * current_residual / total_in;
                if (out_a == out_b) { //don't push to singleton nodes
                    continue;
                }
                bool oa_less_ob = out_a < out_b ? true : false;
                // cout << "i com :" << is_i_common << " j com: " << is_j_common << endl;
                if (!oa_less_ob) {
                    swap(out_a, out_b);
                }
                NodePair pab(out_a, out_b);
                if (!is_i_common) {
                    // i is not common neighbonr
                    push(pab, inc);
                } else {
                    // i is a common neighbor
                    if (is_j_common) {
                        if (oa_less_ob) {
                            push(pab, 2 * inc); // push twice for two commons
                        }
                    } else {// notmal case
                        push(pab, inc);
                    }
                }
            }
        }
    }
    // cout << "------" << endl;
}

void Full_LocalPush::push_to_neighbors(DirectedG &g, NodePair &np, double current_residual) {
    DirectedG::out_edge_iterator
            outi_iter,
            outi_end,
            outj_iter,
            outj_end;

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    for (; outi_iter != outi_end; outi_iter++) {
        auto out_a = target(*outi_iter, g); // out-neighbor
        tie(outj_iter, outj_end) = out_edges(np.second, g);
        for (; outj_iter != outj_end; outj_iter++) {
            auto out_b = target(*outj_iter, g);
            if (out_a == out_b) {
                continue;
            }
            auto indegree_a = in_degree(out_a, g);
            auto indegree_b = in_degree(out_b, g);
            auto total_in = indegree_a * indegree_b;
            NodePair pab(out_a, out_b);
            double inc = c * current_residual / total_in;
            push(pab, inc);
        }
    }
}

double Full_LocalPush::how_much_residual_to_push(DirectedG &g, NodePair &np) {
    return R[np];
}

double Reduced_LocalPush::how_much_residual_to_push(DirectedG &g, NodePair &np) {
    // determine the residual value for current pair to push
    double r = R[np];
    if (np.first == np.second) { //singleton node
        return r - r_max / (1 - c); // singleton nodes do not need to push all residual as 1
        // return r;
        // return 1;
    }
    /* check whether np forms a self-loop */
    if (edge(np.first, np.second, g).second == true &&
        edge(np.second, np.first, g).second == true) { // check whether exists reverse edge
        auto in_deg_a = in_degree(np.first, g);
        auto in_deg_b = in_degree(np.second, g);
        double alpha = c / (in_deg_a * in_deg_b);
        int k = ceil(log(r_max / fabs(r)) / log(alpha));
        double residual_to_push = (1 - pow(alpha, k)) * r / (1 - alpha);
        return residual_to_push;
    } else {
        auto push_residual = r;
        return push_residual;
        // optimize for neighbor-loop
        // double current_max = 0;
        // double max_m = 0;
        // double alpha_i;
        // double alpha_0;
        // DirectedG::out_edge_iterator out_a_it, out_a_end;
        // DirectedG::out_edge_iterator out_b_it, out_b_end;
        // alpha_0 = c / (in_degree(np.first,g) * in_degree(np.second,g));
        // tie(out_a_it, out_a_end) = out_edges(np.first,g);
        // for(;out_a_it != out_a_end; out_a_it ++){
        //     tie(out_b_it, out_b_end) = out_edges(np.second,g);
        //     auto out_a = target(*out_a_it,g);
        //     auto in_a = in_degree(out_a,g);
        //     bool out_a_to_first = edge(out_a,np.first,g).second ;// whether there is a reverse link from  out_a to np.first
        //     for(;out_b_it!=out_b_end;out_b_it ++){
        //         auto out_b = target(*out_b_it,g);
        //         auto in_b = in_degree(out_b,g);
        //         double current_alpha_i = c / (in_a * in_b);
        //         bool out_b_to_second = edge(out_b,np.second,g).second;// whether there is a reverse link from out_b to np.second
        //         if(out_a_to_first && out_b_to_second){ // is a neighbor-loop
        //             NodePair bnp(out_a, out_b);
        //             double m = fabs(R[bnp] + r * current_alpha_i);
        //             if(m > r_max && m > current_max){
        //                 current_max = m;
        //                 alpha_i = current_alpha_i;
        //                 // if(fabs(r_x) > current_max){
        //                 //     current_max = r_x;
        //                 // }
        //             }
        //         }
        //     }
        // }
        // if(current_max > 0){ // if there is any self lop
        //     int k = int(ceil(log(r_max / (alpha_0 * current_max))/ log(alpha_0 * alpha_i))) + 1;
        //     double r_x= 0; // the residual to be pushed
        //     r_x = r + alpha_0 * current_max * (1 - pow(alpha_0 * alpha_i, k)) / (1 - alpha_0 * alpha_i);
        //     // cout << "neighbor loop.." << r_x << endl;
        //     return r_x;
        // }else{
        //     return r;
        // }
    }

    // // non-singleton nodes

    // // compute optimal value to push
    // double sum_airi =  0;
    // double sum_ai_square = 0;
    // DirectedG::out_edge_iterator out_a_it, out_a_end;
    // DirectedG::out_edge_iterator out_b_it, out_b_end;
    // for(;out_a_it != out_a_end; out_a_it ++){
    //     auto out_a = target(*out_a_it,g);
    //     auto in_a = in_degree(out_a,g);
    //     for(;out_b_it!=out_b_end;out_b_it ++){
    //         auto out_b = target(*out_b_it,g);
    //         auto in_b = in_degree(out_b,g);
    //         double alpha = c / (in_a * in_b);
    //         sum_airi += alpha * R[NodePair(out_a, out_b)];
    //         sum_ai_square += pow(alpha,2);
    //     }
    // }
    // r = (r - sum_airi) / (1 + sum_ai_square);

    /* determine the residual to push by computing the maximum overlapping interval */
    // cout << "current r " << r << endl;
    // auto out_deg_a = out_degree(np.first,g);
    // auto out_deg_b = out_degree(np.second,g);
    // auto total_out = out_deg_a * out_deg_b;
    // double start[total_out+1];
    // double end[total_out+1];
    // size_t i = 0;
    // if(edge(np.first,np.second,g).second == true && edge(np.second, np.first,g).second == true){
    //     // self loop
    //     double alpha = c / (in_degree(np.first,g) * in_degree(np.second,g));
    //     start[i] = (r_max -r ) / (alpha - 1);
    //     end[i] = (-r_max - r) / (alpha - 1);
    // }else{
    //     double alpha = c / (in_degree(np.first,g) * in_degree(np.second,g));
    //     start[i] = r-r_max;
    //     end[i] = r + r_max;
    // }

    // i++;

    // DirectedG::out_edge_iterator out_a_it, out_a_end;
    // DirectedG::out_edge_iterator out_b_it, out_b_end;
    // for(;out_a_it != out_a_end; out_a_it ++){
    //     auto out_a = target(*out_a_it,g);
    //     auto in_a = in_degree(out_a,g);
    //     for(;out_b_it!=out_b_end;out_b_it ++){
    //         auto out_b = target(*out_b_it,g);
    //         auto in_b = in_degree(out_b,g);
    //         double alpha = c / (in_a * in_b);
    //         double r_i = R[NodePair(out_a, out_b)];
    //         auto current_start = - (r_max + r_i) / alpha;
    //         auto current_end = (r_max - r_i) / alpha;
    //         if(current_start > end[0] || current_end < start[0]){
    //             // filter out the nodes that conflicts with source node
    //             continue;
    //         }else{
    //             if(current_start < start[0]){
    //                 current_start = start[0];
    //             }
    //             if(current_end > end[0]){
    //                 current_end = end[0];
    //             }
    //             start[i] = current_start;
    //             end[i] = current_end;
    //             i ++;
    //         }
    //     }
    // }
    // // for(i = 0; i< total_out +1;i++){
    // //     int a;
    // //     cout << "lower bound: " << start[i] << " upper bound: "<< end[i] << endl;
    // //     cin >> a;
    // // }

    // double push_residual = findMaxInterval(start, end, i); // i is the length
}

void LocalPush::local_push(DirectedG &g) { // local push given current P and R
    // cout << r_max << endl;
    auto start = std::chrono::high_resolution_clock::now();
    double sum_of_est = 0;

    while (!Q.empty()) {
        NodePair np = Q.front();
        Q.pop();
        marker[np] = false;

        double residual_to_push = how_much_residual_to_push(g, np);
        sum_of_est += residual_to_push;

        R[np] -= residual_to_push;
        P[np] += residual_to_push;
        push_to_neighbors(g, np, residual_to_push); // push residuals to neighbros of np
    }
    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
#ifdef OUTPUT
    if (cpu_time == -1) {
        cpu_time = elapsed.count();
        mem_size = getValue();
    }
#endif
}

void LocalPush::save() {
    // save data to disk
    // save P
    string p_path = get_file_path_base() + ".P";
    P.save(p_path);
    // FILE *outP = fopen(p_path.c_str(), "wb");
    // cout << "saving P to " << p_path << endl;
    // P.serialize(FileSerializer(), outP);
    // fclose(outP);
    //save R
    string r_path = get_file_path_base() + ".R";
    R.save(r_path);
    // FILE *outR = fopen(r_path.c_str(), "wb");
    // cout << "saving R to " << r_path << endl;
    // R.serialize(FileSerializer(), outR);
    // fclose(outR);

    ofstream out;
    // // save meta info
    string meta_path = get_file_path_base() + ".meta";
    out.open(meta_path);
    out << g_name << endl;
    out << n << endl;
    out << epsilon << endl;
    out << c << endl;
    out << cpu_time << endl;
    out << mem_size << endl;
    auto p_size = P.size();
    auto r_size = R.size();
    out << p_size << endl;
    out << r_size << endl;
    out << double(p_size) / (n * n) << endl;
    out << double(r_size) / (n * n) << endl;
    out.close();

    // // save the experimental data
    // string exp_path =get_file_path_base()+".exp";
    // out.open(exp_path);
    // out << n << endl;
    // cout << c << endl;
    // cout << epsilon << endl;
    // out.close();
    // save exp data
    cout << "save complete" << endl;

}

void LocalPush::load() {
    // load existing local push data
    // load P
    string p_path = get_file_path_base() + ".P";
    P.load(p_path);
    // FILE *inP = fopen(p_path.c_str(), "rb");
    // cout << "loading P to " << p_path << endl;
    // P.unserialize(FileSerializer(), inP);
    // fclose(inP);
    //load R
    string r_path = get_file_path_base() + ".R";
    R.load(r_path);
    // FILE *inR = fopen(r_path.c_str(), "rb");
    // cout << "loading R to " << r_path << endl;
    // R.unserialize(FileSerializer(), inR);
    // fclose(inR);

    string meta_path = get_file_path_base() + ".meta";
    ifstream in;
    in.open(meta_path);
    in >> g_name >> n >> epsilon >> c >> cpu_time >> mem_size;
    r_max = cal_rmax(c, epsilon);
    marker.add(n); // initialize marker
    in.close();
}

void LocalPush::show() {
    // cout << P.size() << endl;
    // for(auto &item:P){
    //     cout << "(" << item.first.first << "," << item.first.second << "): " << item.second << endl;
    // }
}

string Reduced_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("RLP_%s-%.3f-%.6f") % g_name % c % epsilon);
}

string Full_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("FLP_%s-%.3f-%.6f") % g_name % c % epsilon);
}

double Full_LocalPush::query_P(unsigned long a, unsigned long b) {
    return P.query(a, b);
}

double Full_LocalPush::query_R(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    return R.query(a, b);
}

void Full_LocalPush::insert(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v, DirectedG &g) {
    DirectedG::vertex_iterator v_it, v_end;
    tie(v_it, v_end) = vertices(g);
    auto in_deg_v = in_degree(v, g);
    for (; v_it != v_end; v_it++) {
        auto a = *v_it;
        if (a != v) {
            update_residual(u, v, a, g, in_deg_v);
        } else {
            // a == v
            R[NodePair(a, a)] = 1 - P.query(a, a);
        }
    }
}

void Full_LocalPush::update_residual(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                                     DirectedG::vertex_descriptor a, DirectedG &g, int in_deg_v) {
    // 1st: accumulate estimates
    DirectedG::in_edge_iterator in_a_it, in_a_end;
    tie(in_a_it, in_a_end) = in_edges(a, g);
    float estimation_sum = 0;
    for (; in_a_it != in_a_end; in_a_it++) {
        auto ina = source(*in_a_it, g);
        estimation_sum += P.query(u, ina);
    }

    // 2nd: update residual
    auto np = NodePair(v, a);
    float left_part = c * estimation_sum / (in_deg_v + 1) / in_degree(a, g);
    float right_part = (P.query(v, a) + R.query(v, a)) / in_deg_v;
    R[np] = left_part - right_part;
    // 3rd: update queue
    if (fabs(R[np]) > r_max) {
        if (!marker[np]) {
            Q.push(np);
            marker[np] = true;
        }
    }
}

void LocalPush::insert(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                       DirectedG &g) {
    // insert edge(u,v) to g, noted we assume g is already updated, and we just update P and R
    DirectedG::vertex_iterator v_it, v_end;
    tie(v_it, v_end) = vertices(g);
    for (; v_it != v_end; v_it++) {
        auto a = *v_it;
//        auto in_deg_v = in_degree(v, g); // the new degree of v
        if (a > v) {
            update_residual(g, v, a);
        } else if (a < v) {
            update_residual(g, a, v);
        } else {
            // a == v
            R[NodePair(a, a)] = 1 - P[NodePair(a, a)];
        }
    }
//    local_push(g);
}

void LocalPush::remove(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                       DirectedG &g) {
    // remove edge(a,b) to g, noted we assume g is already updated, and we just update P and R
}

double LocalPush::query_P(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    if (a > b) {
        return P[NodePair(b, a)];
    } else {
        return P[NodePair(a, b)];
    }
}

double LocalPush::query_R(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    if (a > b) {
        return R[NodePair(b, a)];
    } else {
        return R[NodePair(a, b)];
    }
}

void Reduced_LocalPush::update_residual(DirectedG &g, DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    // cout << "-----" << endl;
    // cout << "updating residual "<< a << " " << b << endl;
    NodePair np(a, b); // a < b
    PairMarker indicator; // the indicator of in-neighbors of (a,b)
    DirectedG::in_edge_iterator in_a_it, in_a_end, in_b_it, in_b_end;
    tie(in_a_it, in_a_end) = in_edges(a, g);
    auto in_deg_a = in_degree(a, g);
    auto in_deg_b = in_degree(b, g);
    if (in_deg_a * in_deg_b > 0) {
//        cout << format("in_deg_a:%s, in_deg_b:%s") % in_deg_a % in_deg_b << endl;
        double sum_neighbor_residuals = 0;
        for (; in_a_it != in_a_end; in_a_it++) {
            tie(in_b_it, in_b_end) = in_edges(b, g);
            for (; in_b_it != in_b_end; in_b_it++) {
                auto ina = source(*in_a_it, g);
                auto inb = source(*in_b_it, g);
                // indicator[NodePair(min(ina,inb), max(ina,inb))] = true;
                sum_neighbor_residuals += P[NodePair(min(ina, inb), max(ina, inb))];
            }
        }
        // for(auto& item:indicator){
        //     // cout << "collect neighbor P (" << item.first.first
        //     //     << "," << item.first.second <<"): " << P[item.first]  << endl;
        //     sum_neighbor_residuals += P[item.first]; // item.first is the node pair
        // }
        R[np] = c * sum_neighbor_residuals / (in_deg_a * in_deg_b) - P[np];
        // cout << "neighbros residuals " << sum_neighbor_residuals << endl;
        // cout << " new residual " << R[np] <<  " its current estimates " << P[np] << endl;
        if (fabs(R[NodePair(a, b)]) > r_max) {
            if (marker[np] == false) {
                Q.push(np);
                marker[np] = true;
            }
        }
    }
}

// dynamic update support: deleting and inserting edge, time complexity: O(E)
void Reduced_LocalPush::update_residuals_by_deleting_edge(DirectedG &g, DirectedG::vertex_descriptor u,
                                                          DirectedG::vertex_descriptor v) {
    // udpate residual when an edge (u,v) is removed from G
    // assumption: G has not been updated, (u,v) exists in G
    // a: the iterated node over G
    // v: the node whose in-neighbor is changed
    DirectedG::vertex_iterator v_begin, v_end, v_it;
    tie(v_begin, v_end) = vertices(g);
    for (v_it = v_begin; v_it != v_end; v_it++) {
        auto a = *v_it;
        if (a != v && in_degree(a, g) != 0) {
            NodePair np = a < v ? NodePair(a, v) : NodePair(v, a); // the node pair to be updated

            DirectedG::in_edge_iterator in_a_iter, in_a_begin, in_a_end;
            double P_av = P.query(np.first, np.second); // current P(a,v)
            auto &R_av_ref = R[np]; // current R(a,v)
            double updated_R = P_av + R_av_ref;

            if (in_degree(v, g) == 1) {
                // u is the only in-neighbor of v
                updated_R = -P_av;
            } else {
                tie(in_a_begin, in_a_end) = in_edges(a, g);
                double u_contrib = 0;
                //iterate over a's in-neighbors
                for (in_a_iter = in_a_begin; in_a_iter != in_a_end; in_a_iter++) {
                    auto a_prime = source(*in_a_iter, g);
                    double contrib = 0;
                    if (a_prime > u) {
                        contrib = c * P.query(u, a_prime);
                    } else if (a_prime < u) {
                        contrib = c * P.query(a_prime, u);
                    } else if (a_prime == u) {
                        contrib = c * sqrt(2) * P.query(a_prime, u);
                    }
                    u_contrib += contrib;
                }
                u_contrib = u_contrib / (in_degree(v, g) * in_degree(a, g));
                updated_R = updated_R - u_contrib;
                updated_R = updated_R * ((in_degree(v, g)) / (in_degree(v, g) - 1.0));
                updated_R -= P_av;
            }
            R_av_ref = updated_R;

            if (fabs(updated_R) / sqrt(2) > r_max) {
                if (!marker[np]) {
                    Q.push(np);
                    marker[np] = true;
                }
            }
        }
    }
}

void Reduced_LocalPush::update_residuals_by_adding_edge(DirectedG &g, DirectedG::vertex_descriptor u,
                                                        DirectedG::vertex_descriptor v) {
    // update the residual when an edge (u,v) is inserted to g
    // node a: the iterated node over G
    // node v: whose in-neighbor has changed (u added)
    // assumption: g has not been updated, (u,v) does not exist in g
    DirectedG::vertex_iterator v_begin, v_end, v_it;
    tie(v_begin, v_end) = vertices(g);
    for (v_it = v_begin; v_it != v_end; v_it++) { // ignore the starting node 0
        auto a = *v_it;

        if (a != v && in_degree(a, g) != 0) {
            double u_contrib = 0; // the amount of R^{'} related to u
            NodePair np = a < v ? NodePair(a, v) : NodePair(v, a); // the node pair to be updated

            auto &residual_ref = R[np];
            auto scaled_rest =
                    (in_degree(v, g) / (in_degree(v, g) + 1.0)) * (residual_ref + P.query(np.first, np.second));

            // iterate over a's in-neighbors
            DirectedG::in_edge_iterator in_a_iter, in_a_begin, in_a_end;
            tie(in_a_begin, in_a_end) = in_edges(a, g);
            for (in_a_iter = in_a_begin; in_a_iter != in_a_end; in_a_iter++) {
                auto a_prime = source(*in_a_iter, g);
                double contrib = 0;
                if (a_prime > u) {
                    contrib = c * P.query(u, a_prime);
                } else if (a_prime < u) {
                    contrib = c * P.query(a_prime, u);
                } else if (a_prime == u) {
                    contrib = c * sqrt(2) * P.query(a_prime, u);
                }
                u_contrib += contrib;
            }
            u_contrib = u_contrib / ((in_degree(v, g) + 1.0) * in_degree(a, g));

            auto updated_residual = u_contrib + scaled_rest - P.query(np.first, np.second);
            residual_ref = updated_residual;

            if (fabs(updated_residual) / sqrt(2) > r_max) {
                if (!marker[np]) {
                    Q.push(np);
                    marker[np] = true;
                }
            }
        }
    }
}

void Reduced_LocalPush::update_edges(DirectedG &g, vector<NodePair> edges, char update_type = '+') {
    // update G by a set of edges
    if (update_type == '+') {
        for (auto &e: edges) {
            int s = e.first;
            int t = e.second;
            update_residuals_by_adding_edge(g, s, t);
            add_edge(s, t, g);
        }
    } else if (update_type == '-') {
        for (auto &e: edges) {
            int s = e.first;
            int t = e.second;

            if (edge(s, t, g).second) {
                update_residuals_by_deleting_edge(g, s, t);
                remove_edge(s, t, g);
            }
        }
    } else {
        cout << "Please indicate the edge update type" << endl;
        return;
    }
    log_info("finish updating residuals...");
    Reduced_LocalPush::local_push(g);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <string>
#include <unordered_map>

#include "ControlSystem/FunctionOfTime.hpp"
#include "ControlSystem/PiecewisePolynomial.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/CubicScale.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/StdArrayHelpers.hpp"
#include "Utilities/TypeTraits.hpp"
#include "tests/Unit/Domain/CoordinateMaps/TestMapHelpers.hpp"
#include "tests/Unit/TestHelpers.hpp"

namespace {
// We will call with this function with arguments that are designed
// to fail in a certain way.
void cubic_scale_non_invertible(const double a0, const double b0,
                                const double outer_boundary) noexcept {
  double t = 4.0;
  constexpr size_t deriv_order = 2;

  const std::array<DataVector, deriv_order + 1> init_func_a{
      {{a0}, {0.0}, {0.0}}};
  FunctionsOfTime::PiecewisePolynomial<deriv_order> expansion_a(t, init_func_a);
  FunctionOfTime& expansion_a_base = expansion_a;

  const std::array<DataVector, deriv_order + 1> init_func_b{
      {{b0}, {0.0}, {0.0}}};
  FunctionsOfTime::PiecewisePolynomial<deriv_order> expansion_b(t, init_func_b);
  FunctionOfTime& expansions_b_base = expansion_b;

  const std::unordered_map<std::string, FunctionOfTime&> f_of_t_list = {
      {"expansion_a", expansion_a_base}, {"expansion_b", expansions_b_base}};

  const CoordMapsTimeDependent::CubicScale scale_map(outer_boundary);
  const std::array<double, 1> point_xi{{19.2}};

  const std::array<double, 1> mapped_point{
      {point_xi[0] *
       (expansion_a_base.func(t)[0][0] +
        (expansions_b_base.func(t)[0][0] - expansion_a_base.func(t)[0][0]) *
            square(point_xi[0] / outer_boundary))}};

  // this call should fail.
  scale_map.inverse(mapped_point, t, f_of_t_list);
}
}  // namespace

SPECTRE_TEST_CASE("Unit.Domain.CoordMapsTimeDependent.CubicScale",
                  "[Domain][Unit]") {
  static constexpr size_t deriv_order = 2;

  const auto run_tests = [](
      const std::array<DataVector, deriv_order + 1>& init_func_a,
      const std::array<DataVector, deriv_order + 1>& init_func_b,
      const double outer_b, const double x0, const double final_time) {
    double t = -0.5;
    const double dt = 0.6;

    FunctionsOfTime::PiecewisePolynomial<deriv_order> expansion_a(t,
                                                                  init_func_a);
    FunctionOfTime& expansion_a_base = expansion_a;

    FunctionsOfTime::PiecewisePolynomial<deriv_order> expansion_b(t,
                                                                  init_func_b);
    FunctionOfTime& expansions_b_base = expansion_b;

    const std::unordered_map<std::string, FunctionOfTime&> f_of_t_list = {
        {"expansion_a", expansion_a_base}, {"expansion_b", expansions_b_base}};

    const double outer_boundary = outer_b;
    const CoordMapsTimeDependent::CubicScale scale_map(outer_boundary);
    const std::array<double, 1> point_xi{{x0}};

    // test serialized/deserialized map
    const auto scale_map_deserialized = serialize_and_deserialize(scale_map);

    while (t < final_time) {
      const double a = expansion_a_base.func_and_deriv(t)[0][0];
      const double b = expansions_b_base.func_and_deriv(t)[0][0];

      const std::array<double, 1> mapped_point{
          {point_xi[0] * (a + (b - a) * square(point_xi[0] / outer_boundary))}};

      CHECK_ITERABLE_APPROX(scale_map(point_xi, t, f_of_t_list), mapped_point);
      CHECK_ITERABLE_APPROX(
          scale_map.inverse(mapped_point, t, f_of_t_list).get(), point_xi);
      CHECK_ITERABLE_APPROX(scale_map_deserialized(point_xi, t, f_of_t_list),
                            mapped_point);
      CHECK_ITERABLE_APPROX(
          scale_map_deserialized.inverse(mapped_point, t, f_of_t_list).get(),
          point_xi);

      // Check that inverse map returns invalid for mapped point outside
      // the outer boundary and inside the inner boundary.
      const std::array<double, 1> bad_mapped_point_1{{1.1 * outer_boundary}};
      const std::array<double, 1> bad_mapped_point_2{{-0.1 * outer_boundary}};
      CHECK(not scale_map.inverse(bad_mapped_point_1, t, f_of_t_list));
      CHECK(not scale_map.inverse(bad_mapped_point_2, t, f_of_t_list));

      const double jacobian =
          a + 3.0 * (b - a) * square(point_xi[0] / outer_boundary);
      const double inv_jacobian =
          1.0 / (a + 3.0 * (b - a) * square(point_xi[0] / outer_boundary));

      CHECK(scale_map.jacobian(point_xi, t, f_of_t_list).get(0, 0) == jacobian);
      CHECK(scale_map.inv_jacobian(point_xi, t, f_of_t_list).get(0, 0) ==
            inv_jacobian);
      CHECK(
          scale_map_deserialized.jacobian(point_xi, t, f_of_t_list).get(0, 0) ==
          jacobian);
      CHECK(scale_map_deserialized.inv_jacobian(point_xi, t, f_of_t_list)
                .get(0, 0) == inv_jacobian);

      const double a_dot = expansion_a_base.func_and_deriv(t)[1][0];
      const double b_dot = expansions_b_base.func_and_deriv(t)[1][0];
      const std::array<double, 1> frame_vel =
          point_xi *
          (a_dot + (b_dot - a_dot) * square(point_xi[0] / outer_boundary));

      CHECK_ITERABLE_APPROX(scale_map.frame_velocity(point_xi, t, f_of_t_list),
                            frame_vel);
      CHECK_ITERABLE_APPROX(
          scale_map_deserialized.frame_velocity(point_xi, t, f_of_t_list),
          frame_vel);

      const double a_ddot = expansion_a_base.func_and_2_derivs(t)[2][0];
      const double b_ddot = expansions_b_base.func_and_2_derivs(t)[2][0];
      const double time_time =
          a_ddot * point_xi[0] +
          (b_ddot - a_ddot) * cube(point_xi[0]) / square(outer_boundary);
      const double time_space =
          a_dot + 3.0 * (b_dot - a_dot) * square(point_xi[0] / outer_boundary);
      const double space_space =
          6.0 * (b - a) * point_xi[0] / square(outer_boundary);

      CHECK(approx(scale_map.hessian(point_xi, t, f_of_t_list).get(0, 0, 0)) ==
            time_time);
      CHECK(approx(scale_map.hessian(point_xi, t, f_of_t_list).get(0, 1, 0)) ==
            time_space);
      CHECK(approx(scale_map.hessian(point_xi, t, f_of_t_list).get(0, 0, 1)) ==
            time_space);
      CHECK(approx(scale_map.hessian(point_xi, t, f_of_t_list).get(0, 1, 1)) ==
            space_space);

      CHECK(approx(scale_map_deserialized.hessian(point_xi, t, f_of_t_list)
                       .get(0, 0, 0)) == time_time);
      CHECK(approx(scale_map_deserialized.hessian(point_xi, t, f_of_t_list)
                       .get(0, 1, 0)) == time_space);
      CHECK(approx(scale_map_deserialized.hessian(point_xi, t, f_of_t_list)
                   .get(0, 0, 1)) == time_space);
      CHECK(approx(scale_map_deserialized.hessian(point_xi, t, f_of_t_list)
                       .get(0, 1, 1)) == space_space);

      // Check inequivalence operator
      CHECK_FALSE(scale_map != scale_map);
      CHECK_FALSE(scale_map_deserialized != scale_map_deserialized);

      // Check serialization
      CHECK(scale_map == scale_map_deserialized);
      CHECK_FALSE(scale_map != scale_map_deserialized);

      test_coordinate_map_argument_types(scale_map, point_xi, t, f_of_t_list);

      t += dt;
    }
  };

  std::array<DataVector, deriv_order + 1> init_func_a1{
      {{1.0}, {0.0007}, {-0.004}}};
  std::array<DataVector, deriv_order + 1> init_func_b1{
      {{1.0}, {-0.001}, {0.003}}};
  run_tests(init_func_a1, init_func_b1, 20.0, 19.2, 4.0);
  // test again with inputs that explicitly result in multiple roots
  std::array<DataVector, deriv_order + 1> init_func_a2{{{1.0}, {0.0}, {0.0}}};
  std::array<DataVector, deriv_order + 1> init_func_b2{{{0.99}, {0.0}, {0.0}}};
  run_tests(init_func_a2, init_func_b2, 6.0, 5.0, 0.0);
}

SPECTRE_TEST_CASE(
    "Unit.Domain.CoordMapsTimeDependent.CubicScale.TestBoundaries",
    "[Domain][Unit]") {
  static constexpr size_t deriv_order = 2;

  const auto run_tests = [](const double x0) {
    double t = -0.5;
    const double dt = 0.6;
    const double final_time = 4.0;

    const std::array<DataVector, deriv_order + 1> init_func_a{
        {{1.0}, {0.0}, {0.0}}};
    const std::array<DataVector, deriv_order + 1> init_func_b{
        {{0.99}, {0.0}, {0.0}}};

    FunctionsOfTime::PiecewisePolynomial<deriv_order> expansion_a(t,
                                                                  init_func_a);
    FunctionOfTime& expansion_a_base = expansion_a;

    FunctionsOfTime::PiecewisePolynomial<deriv_order> expansion_b(t,
                                                                  init_func_b);
    FunctionOfTime& expansions_b_base = expansion_b;

    const std::unordered_map<std::string, FunctionOfTime&> f_of_t_list = {
        {"expansion_a", expansion_a_base}, {"expansion_b", expansions_b_base}};

    const double outer_boundary = 20.0;
    const CoordMapsTimeDependent::CubicScale scale_map(outer_boundary);
    const std::array<double, 1> point_xi{{x0}};

    while (t < final_time) {
      const double a = expansion_a_base.func_and_deriv(t)[0][0];
      const double b = expansions_b_base.func_and_deriv(t)[0][0];

      const std::array<double, 1> mapped_point{
          {point_xi[0] * (a + (b - a) * square(point_xi[0] / outer_boundary))}};

      CHECK_ITERABLE_APPROX(scale_map(point_xi, t, f_of_t_list), mapped_point);
      CHECK_ITERABLE_APPROX(
          scale_map.inverse(mapped_point, t, f_of_t_list).get(), point_xi);
      t += dt;
    }
  };

  // test inverse at inner and outer boundary values
  run_tests(0.0);
  run_tests(20.0);
}

// [[OutputRegex, The map is invertible only if 0 < expansion_b <
// expansion_a\*2/3]]
SPECTRE_TEST_CASE(
    "Unit.Domain.CoordMapsTimeDependent.CubicScale.NonInvertible1",
    "[Domain][Unit]") {
  ERROR_TEST();
  // the two expansion factors are chosen such that the map is non-invertible
  cubic_scale_non_invertible(0.96, 0.58, 20.0);
}

// [[OutputRegex, We require expansion_a > 0 for invertibility]]
SPECTRE_TEST_CASE(
    "Unit.Domain.CoordMapsTimeDependent.CubicScale.NonInvertible2",
    "[Domain][Unit]") {
  ERROR_TEST();
  // Make a<0
  cubic_scale_non_invertible(-0.0001, 1.0, 20.0);
}

// [[OutputRegex, The map is invertible only if 0 < expansion_b <
// expansion_a\*2/3]]
SPECTRE_TEST_CASE(
    "Unit.Domain.CoordMapsTimeDependent.CubicScale.NonInvertible3",
    "[Domain][Unit]") {
  ERROR_TEST();
  // Make b==0
  cubic_scale_non_invertible(0.96, 0.0, 20.0);
}

// [[OutputRegex, For invertability, we require outer_boundary to be positive]]
SPECTRE_TEST_CASE(
    "Unit.Domain.CoordMapsTimeDependent.CubicScale.NonInvertible4",
    "[Domain][Unit]") {
  ERROR_TEST();
  cubic_scale_non_invertible(0.96, 1.0, 0.0);
}


// Copyright (C) 2014-2016 Vicente J. Botet Escriba
//
//  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)

// <experimental/functor.hpp>
// <experimental/utility.hpp> tuple

#include <experimental/array.hpp>
#include <experimental/functor.hpp>
#include <experimental/product_type.hpp>

#include <boost/detail/lightweight_test.hpp>

int twice(int i) {
  return 2*i;
}

int main()
{
#if __cplusplus >= 201402L
  namespace stde = std::experimental;

  using T = std::array<int, 2>;

  static_assert(stde::is_product_type<T>::value, "ERROR");
  static_assert(stde::is_type_constructible<T>::value, "ERROR");
  static_assert(stde::is_functor<stde::array_tc>::value, "ERROR");

  {
    int v=1;
    T x = std::make_array(v,v);
    BOOST_TEST(std::get<0>(x) == 1);
    BOOST_TEST(std::get<1>(x) == 1);
    T y = stde::functor::transform(x, twice);
    BOOST_TEST(std::get<0>(y) == 2);
    BOOST_TEST(std::get<1>(y) == 2);
  }
#endif

  return ::boost::report_errors();
}


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/assign/list_of.hpp>
using boost::assign::list_of;

#include <evaluation/util/ConfusionMatrixUtil.h>
using namespace evaluation;

//#################### TYPEDEFS ####################

typedef int Label;

//#################### TESTS ####################

BOOST_AUTO_TEST_SUITE(test_ConfusionMatrixUtil)

BOOST_AUTO_TEST_CASE(get_conf_mtx_test)
{
  // Generate a set of labels;
  std::set<Label> classLabels = list_of(2)(5)(1)(9);

  // Generate dummy ground truth and predicted labels.
  std::vector<Label> groundTruthLabels = list_of(1)(5)(9)(1)(5)(9)(1)(5)(9)(1)(5)(9)(1)(5)(9);
  std::vector<Label> predictedLabels = list_of(1)(2)(5)(1)(2)(5)(1)(2)(5)(1)(2)(5)(1)(2)(5);

  // Generate a confusion matrix.
  Eigen::MatrixXf testConfMtx = ConfusionMatrixUtil::make_confusion_matrix(classLabels, groundTruthLabels, predictedLabels);

  // The std::set orders the integers from smallest to largest.
  // The ground truth examples will be [1 5 9 1 5 9 ... 1 5 9]
  // The predicted labels will be      [1 2 5 1 2 5 ... 1 2 5]
  // Therefore the 1's are all in the correct place, i.e. trueConfMtx(0,0) = 5.0f.
  // All the 5's are predicted as 2's.
  // Since the 5's are in the 3rd position in classLabels,
  // and the 2's are in the 2nd position,
  // we expect the all the pairs of labels [gt=5 pred=2]
  // to be at position (3-1,2-1), i.e. trueConfMtx(2,1) = 5.0f.
  Eigen::MatrixXf trueConfMtx = Eigen::MatrixXf::Zero(4,4);
  trueConfMtx(0,0) = 5.0f;
  trueConfMtx(2,1) = 5.0f;
  trueConfMtx(3,2) = 5.0f;

  BOOST_CHECK_EQUAL(testConfMtx, trueConfMtx);

  Eigen::MatrixXf testConfMtxNormL1 = ConfusionMatrixUtil::normalise_rows_L1(trueConfMtx);
  Eigen::MatrixXf trueConfMtxNormL1 = Eigen::MatrixXf::Zero(4,4);
  trueConfMtxNormL1(0,0) = 1.0f;
  trueConfMtxNormL1(2,1) = 1.0f;
  trueConfMtxNormL1(3,2) = 1.0f;

  BOOST_CHECK_EQUAL(testConfMtxNormL1, trueConfMtxNormL1);
}

BOOST_AUTO_TEST_SUITE_END()


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2010 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "sparse.h"
#include <Eigen/SparseExtra>

#ifdef EIGEN_CHOLMOD_SUPPORT
#include <Eigen/CholmodSupport>
#endif

template<typename Scalar> void sparse_llt(int rows, int cols)
{
  double density = (std::max)(8./(rows*cols), 0.01);
  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
  typedef Matrix<Scalar,Dynamic,1> DenseVector;

    // TODO fix the issue with complex (see SparseLLT::solveInPlace)
    SparseMatrix<Scalar> m2(rows, cols);
    DenseMatrix refMat2(rows, cols);

    DenseVector b = DenseVector::Random(cols);
    DenseVector ref_x(cols), x(cols);
    DenseMatrix B = DenseMatrix::Random(rows,cols);
    DenseMatrix ref_X(rows,cols), X(rows,cols);

    initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, 0, 0);

    for(int i=0; i<rows; ++i)
      m2.coeffRef(i,i) = refMat2(i,i) = internal::abs(internal::real(refMat2(i,i)));

    ref_x = refMat2.template selfadjointView<Lower>().llt().solve(b);
    if (!NumTraits<Scalar>::IsComplex)
    {
      x = b;
      SparseLLT<SparseMatrix<Scalar> > (m2).solveInPlace(x);
      VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: default");
    }

#ifdef EIGEN_CHOLMOD_SUPPORT
    // legacy API
    {
      // Cholmod, as configured in CholmodSupport.h, only supports self-adjoint matrices
      SparseMatrix<Scalar> m3 = m2.adjoint()*m2;
      DenseMatrix refMat3 = refMat2.adjoint()*refMat2;

      ref_x = refMat3.template selfadjointView<Lower>().llt().solve(b);

      x = b;
      SparseLLT<SparseMatrix<Scalar>, Cholmod>(m3).solveInPlace(x);
      VERIFY((m3*x).isApprox(b,test_precision<Scalar>()) && "LLT legacy: cholmod solveInPlace");

      x = SparseLLT<SparseMatrix<Scalar>, Cholmod>(m3).solve(b);
      VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT legacy: cholmod solve");
    }

    // new API
    {
      // Cholmod, as configured in CholmodSupport.h, only supports self-adjoint matrices
      SparseMatrix<Scalar> m3 = m2 * m2.adjoint(), m3_lo(rows,rows), m3_up(rows,rows);
      DenseMatrix refMat3 = refMat2 * refMat2.adjoint();

      m3_lo.template selfadjointView<Lower>().rankUpdate(m2,0);
      m3_up.template selfadjointView<Upper>().rankUpdate(m2,0);

      // with a single vector as the rhs
      ref_x = refMat3.template selfadjointView<Lower>().llt().solve(b);

      x = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3).solve(b);
      VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");

      x = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3).solve(b);
      VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");

      x = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3_lo).solve(b);
      VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");

      x = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3_up).solve(b);
      VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "LLT: cholmod solve, single dense rhs");

      // with multiple rhs
      ref_X = refMat3.template selfadjointView<Lower>().llt().solve(B);

      #ifndef EIGEN_DEFAULT_TO_ROW_MAJOR
      // TODO make sure the API is properly documented about this fact
      X = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3).solve(B);
      VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "LLT: cholmod solve, multiple dense rhs");

      X = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3).solve(B);
      VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "LLT: cholmod solve, multiple dense rhs");
      #endif

      // with a sparse rhs
      SparseMatrix<Scalar> spB(rows,cols), spX(rows,cols);
      B.diagonal().array() += 1;
      spB = B.sparseView(0.5,1);

      ref_X = refMat3.template selfadjointView<Lower>().llt().solve(DenseMatrix(spB));

      spX = CholmodDecomposition<SparseMatrix<Scalar>, Lower>(m3).solve(spB);
      VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");

      spX = CholmodDecomposition<SparseMatrix<Scalar>, Upper>(m3).solve(spB);
      VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");
    }
#endif

}

void test_sparse_llt()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(sparse_llt<double>(8, 8) );
    int s = internal::random<int>(1,300);
    CALL_SUBTEST_2(sparse_llt<std::complex<double> >(s,s) );
    CALL_SUBTEST_1(sparse_llt<double>(s,s) );
  }
}


#define _USE_MATH_DEFINES

#include "scenepic.h"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <tuple>

namespace sp = scenepic;

std::tuple<sp::Vector, sp::Vector> get_bounds(const sp::VectorBuffer& verts)
{
  sp::Vector vert_max = verts.colwise().maxCoeff();
  sp::Vector vert_min = verts.colwise().minCoeff();
  return std::make_tuple(0.5 * (vert_min + vert_max), vert_max - vert_min);
}

/**
 *  This class will create the animation during its initialization.
 *  This is one of many potential patterns to use when constructing more complex
 *  visualizations. The main advantage is that you can pass all configuration
 *  information into the constructor, and then share variables between
 *  different stages of the visualization or animation. This animation
 *  has three stages:
 *
 *  1. Point cloud converging to four cubes
 *  2. Four cubes morphing into the ScenePic logo
 *  3. Still shot on the logo
 *
 *  Each is constructed in a separate method, but using shared elements.
 *  One element used in this particular animation is a pre-computed camera
 *  track. While the user can still move the camera around freely,
 *  this can be helpful for create videos from the SVT (e.g. the animated
 *  GIF in the README.)
 */
class Animation
{
public:
  /** Constructor.
   *
   *  \param scene The ScenePic scene.
   *  \param num_cloud_frames Length of cloud stage
   *  \param num_morph_frames Length of morph stage
   *  \param num_still_frames Length of still stage
   *  \param start_distance Starting camera distance
   *  \param end_distance Ending camera distance.
   *  \param start_angles Starting camera angles.
   *  \param end_angles: Ending camera angles.
   *  \param width: Width of the viewport.
   *  \param height Height of the viewport.
   *  \param num_points Size of the point cloud.
   */
  Animation(
    int num_cloud_frames = 30,
    int num_morph_frames = 60,
    int num_still_frames = 30,
    float start_distance = 0.1,
    float end_distance = 2,
    const sp::Vector& start_angles = {1.5, 1, -0.1},
    const sp::Vector& end_angles = {-0.8, -0.2, 0},
    int width = 1280,
    int height = 640,
    int num_points = 1000)
  : m_num_cloud_frames(num_cloud_frames),
    m_num_morph_frames(num_morph_frames),
    m_num_still_frames(num_still_frames),
    m_num_frames(num_cloud_frames + num_morph_frames + num_still_frames),
    m_aspect_ratio(static_cast<float>(width) / static_cast<float>(height))
  {
    m_colors = {
      sp::Color::from_bytes(242, 80, 34),
      sp::Color::from_bytes(127, 186, 0),
      sp::Color::from_bytes(0, 164, 239),
      sp::Color::from_bytes(255, 185, 0)};

    m_canvas = m_scene.create_canvas_3d("main", width, height);

    // load the text meshes
    auto scene_obj = sp::load_obj("scene.obj");
    m_scene_mesh = m_scene.create_mesh("scene_text", "text");
    m_scene_mesh->shared_color(m_colors[0]);
    m_scene_mesh->add_mesh(scene_obj);
    m_scene_mesh->apply_transform(
      sp::Transforms::rotation_about_x(static_cast<float>(M_PI / 2)));
    m_scene_pos = {0, 0, 0};

    auto pic_obj = sp::load_obj("pic.obj");
    m_pic_mesh = m_scene.create_mesh("pic_text", "text");
    m_pic_mesh->shared_color(m_colors[3]);
    m_pic_mesh->add_mesh(pic_obj);
    m_pic_pos = {1.25, -0.05, 0.5};

    // create the cubes for morphing
    std::transform(
      m_colors.begin(),
      m_colors.end(),
      std::back_inserter(m_cubes),
      [this](const sp::Color& color) { return this->create_cube(color); });

    m_cube1_pos = {2, 0.15, 0.25};
    m_cube2_pos = {0.4, -0.05, 0.3};
    m_cube2_scale = {1.5, 0.2, 0.8};
    m_focus_point = {1, 0.25, 0.5};

    // compute the angles and distances for the cameras
    // the camera is parametrized as a "look-at" camera,
    // with a Z-distance from the focus point that is rotated
    // using the provided angles to get a center, and
    // then focused on a set point.
    std::tie(m_angles, m_distances) = compute_camera_info(
      start_distance, end_distance, start_angles, end_angles);

    m_index = 0;
    animate_cloud(num_points);
    animate_morph();
    animate_still();
  }

  /** Save the animation to the specified path.
   *
   *  \param path path to the HTML output file
   */
  void save(const std::string& path)
  {
    m_scene.save_as_html(path, "Hello Scenepic!");
  }

private:
  std::tuple<sp::VectorBuffer, sp::ValueBuffer> compute_camera_info(
    float start_distance,
    float end_distance,
    const sp::Vector& start_angles,
    const sp::Vector& end_angles)
  {
    sp::VectorBuffer angles(m_num_frames, 3);
    angles.topRows(m_num_cloud_frames) = sp::linspace<sp::VectorBuffer>(
      m_num_cloud_frames, start_angles, sp::Vector(0, 0, 0));
    angles.middleRows(m_num_cloud_frames, m_num_morph_frames) =
      sp::linspace<sp::VectorBuffer>(
        m_num_morph_frames, sp::Vector(0, 0, 0), end_angles);
    angles.bottomRows(m_num_still_frames) =
      end_angles.colwise().replicate(m_num_still_frames);

    sp::ValueBuffer distances(m_num_frames);
    distances.fill(end_distance);
    distances.topRows(m_num_cloud_frames)
      .setLinSpaced(start_distance, end_distance);

    return std::make_tuple(angles, distances);
  }

  sp::Camera create_camera(const sp::Vector& focus_point)
  {
    // compute the rotation matrix
    auto rotation =
      sp::Transforms::euler_angles_to_matrix(m_angles.row(m_index));

    // create a vector at the current distance
    sp::Vector pos(0, 0, m_distances(m_index));

    // rotate and offset from the focus point
    pos = rotation.topLeftCorner(3, 3) * pos.transpose();
    pos += focus_point;

    // NB each call to this function will move the camera long its track
    m_index += 1;

    sp::Camera camera(pos, focus_point);
    camera.aspect_ratio(m_aspect_ratio);
    return camera;
  }

  void animate_cloud(int num_points)
  {
    const int num_frames = m_num_cloud_frames;

    // start with random positions
    sp::VectorBuffer start_positions =
      sp::random<sp::VectorBuffer>(num_points, 3, -1, 1);
    sp::VectorBuffer end_positions = sp::VectorBuffer::Zero(num_points, 3);

    // group the points by which will go to each cube
    int group_size = num_points / 4;
    int final_group_size = num_points - (3 * group_size);

    // distribute the final points randomly within each target cube
    end_positions.topRows(group_size) = sp::random<sp::VectorBuffer>(
      group_size,
      3,
      sp::Vector(-0.51, 0.03, -0.22),
      sp::Vector(-0.03, 0.51, 0.22));
    end_positions.middleRows(group_size, group_size) =
      sp::random<sp::VectorBuffer>(
        group_size,
        3,
        sp::Vector(0.03, 0.03, -0.22),
        sp::Vector(0.51, 0.51, 0.22));
    end_positions.middleRows(2 * group_size, group_size) =
      sp::random<sp::VectorBuffer>(
        group_size,
        3,
        sp::Vector(-0.51, -0.51, -0.22),
        sp::Vector(-0.03, -0.03, 0.22));
    end_positions.bottomRows(final_group_size) = sp::random<sp::VectorBuffer>(
      final_group_size,
      3,
      sp::Vector(0.03, -0.51, -0.22),
      sp::Vector(0.51, -0.03, 0.22));

    // set the cloud colors to correspond to the target cubes
    sp::ColorBuffer colors(num_points, 3);
    colors.topRows(group_size) =
      m_colors[0].rowwise().replicate(group_size).transpose();
    colors.middleRows(group_size, group_size) =
      m_colors[1].rowwise().replicate(group_size).transpose();
    colors.middleRows(2 * group_size, group_size) =
      m_colors[2].rowwise().replicate(group_size).transpose();
    colors.bottomRows(final_group_size) =
      m_colors[3].rowwise().replicate(final_group_size).transpose();

    sp::VectorBuffer pos_diff = (end_positions - start_positions) / num_frames;
    sp::VectorBuffer positions = start_positions;

    for (int i = 0; i < num_frames; ++i, positions += pos_diff)
    {
      auto mesh = m_scene.create_mesh("", "cloud");
      mesh->add_cube(sp::Colors::White, sp::Transforms::scale(0.05));
      mesh->enable_instancing(positions, sp::QuaternionBufferNone(), colors);

      auto frame = m_canvas->create_frame();
      frame->add_mesh(mesh);
      frame->camera(create_camera({0, 0, 0}));
    }
  }

  void animate_morph()
  {
    const int num_frames = m_num_morph_frames;

    // because the text will be hiding within the cubes
    // and then eventually revealed, we need to compute
    // some bounds information to position it correctly

    sp::Vector scene_pos, scene_scale;
    std::tie(scene_pos, scene_scale) =
      get_bounds(m_scene_mesh->vertex_positions());
    sp::VectorBuffer scene_positions(num_frames, 3);
    sp::linspace(scene_positions, sp::Vector(-0.47, 0.07, 0), m_scene_pos);
    sp::VectorBuffer scene_scales(num_frames, 3);
    sp::Vector start_scale = 0.4 * scene_scale.cwiseInverse();
    sp::linspace(scene_scales, start_scale, sp::Vector(1, 1, 1));
    scene_scales.col(2).array() = 1;

    sp::Vector pic_pos, pic_scale;
    std::tie(pic_pos, pic_scale) = get_bounds(m_pic_mesh->vertex_positions());
    sp::VectorBuffer pic_positions = sp::linspace<sp::VectorBuffer>(
      num_frames, sp::Vector(0.27, -0.27, pic_scale.z() * 0.5), m_pic_pos);
    start_scale = 0.4 * pic_scale.cwiseInverse();
    sp::VectorBuffer pic_scales = sp::linspace<sp::VectorBuffer>(
      num_frames, start_scale, sp::Vector(1, 1, 1));
    pic_pos = m_pic_pos;
    pic_pos.z() = pic_pos.z() - 0.5 * pic_scale.z();

    // create the animated positions and scales for the cubes
    std::vector<sp::VectorBuffer> positions = {
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(-0.27, 0.27, 0), scene_pos),
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(0.27, 0.27, 0), m_cube1_pos),
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(-0.27, -0.27, 0), m_cube2_pos),
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(0.27, -0.27, 0), pic_pos)};

    scene_scale *= 2;
    pic_scale *= 2;
    std::vector<sp::VectorBuffer> scales = {
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(1, 1, 1), scene_scale),
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(1, 1, 1), sp::Vector(1, 1, 1)),
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(1, 1, 1), m_cube2_scale),
      sp::linspace<sp::VectorBuffer>(
        num_frames, sp::Vector(1, 1, 1), pic_scale)};

    scales[0].col(2).setLinSpaced(1.0f, 0.0f);
    scales[3].col(1).setLinSpaced(1.0f, 0.0f);

    sp::VectorBuffer focus_points = sp::linspace<sp::VectorBuffer>(
      num_frames, sp::Vector(0, 0, 0), m_focus_point);

    for (int f = 0; f < num_frames; ++f)
    {
      auto frame = m_canvas->create_frame();

      sp::Transform transform;
      for (int i = 0; i < 4; ++i)
      {
        transform = sp::Transforms::translate(positions[i].row(f)) *
          sp::Transforms::scale(scales[i].row(f));
        frame->add_mesh(m_cubes[i], transform);
      }

      transform = sp::Transforms::translate(scene_positions.row(f)) *
        sp::Transforms::scale(scene_scales.row(f));
      frame->add_mesh(m_scene_mesh, transform);

      transform = sp::Transforms::translate(pic_positions.row(f)) *
        sp::Transforms::scale(pic_scales.row(f));
      frame->add_mesh(m_pic_mesh, transform);
      frame->camera(create_camera(focus_points.row(f)));
      frame->focus_point(sp::FocusPoint(focus_points.row(f)));
    }
  }

  std::shared_ptr<sp::Mesh> create_cube(const sp::Color& color)
  {
    auto cube = m_scene.create_mesh("", "cubes");
    cube->shared_color(color);
    cube->add_cube(sp::Color::None(), sp::Transforms::scale(0.5));
    return cube;
  }

  void animate_still()
  {
    // this creates the ScenePic logo
    std::vector<std::shared_ptr<sp::Mesh>> meshes = {
      m_scene_mesh, m_cubes[1], m_cubes[2], m_pic_mesh};
    std::vector<sp::Transform> transforms = {
      sp::Transforms::translate(m_scene_pos),
      sp::Transforms::translate(m_cube1_pos),
      sp::Transforms::translate(m_cube2_pos) *
        sp::Transforms::scale(m_cube2_scale),
      sp::Transforms::translate(m_pic_pos)};

    for (int f = 0; f < m_num_still_frames; ++f)
    {
      auto frame = m_canvas->create_frame();

      for (int i = 0; i < 4; ++i)
      {
        frame->add_mesh(meshes[i], transforms[i]);
      }

      frame->camera(create_camera(m_focus_point));
      frame->focus_point(sp::FocusPoint(m_focus_point));
    }
  }

  int m_num_cloud_frames;
  int m_num_morph_frames;
  int m_num_still_frames;
  int m_num_frames;
  float m_aspect_ratio;
  std::vector<sp::Color> m_colors;
  sp::Scene m_scene;
  std::shared_ptr<sp::Canvas3D> m_canvas;
  std::shared_ptr<sp::Mesh> m_scene_mesh;
  sp::Vector m_scene_pos;
  std::shared_ptr<sp::Mesh> m_pic_mesh;
  sp::Vector m_pic_pos;
  std::vector<std::shared_ptr<sp::Mesh>> m_cubes;
  sp::Vector m_cube1_pos;
  sp::Vector m_cube2_pos;
  sp::Vector m_cube2_scale;
  sp::Vector m_focus_point;
  sp::VectorBuffer m_angles;
  sp::ValueBuffer m_distances;
  int m_index;
};

int main(int argc, char* argv[])
{
  Animation anim;
  anim.save("hello_scenepic.html");
}


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_MODELS_DYNAMIC_INTEGRATION_HPP_
#define BARK_MODELS_DYNAMIC_INTEGRATION_HPP_
#include <Eigen/Core>

#include "bark/geometry/angle.hpp"
#include "bark/models/dynamic/dynamic_model.hpp"

namespace bark {
namespace models {
namespace dynamic {

inline State euler_int(const DynamicModel& model,
                const State &x,
                const Input &u,
                float dt) {
  State new_x = x + dt * model.StateSpaceModel(x, u);
  new_x(StateDefinition::THETA_POSITION) =
      geometry::Norm0To2PI(new_x(StateDefinition::THETA_POSITION));
  return new_x;
}

inline State rk4(const DynamicModel& model, const State& x, const Input& u,
                 float dt) {
  State k0 = dt * model.StateSpaceModel(x, u);
  State k1 = dt * model.StateSpaceModel(x + k0 / 2, u);
  State k2 = dt * model.StateSpaceModel(x + k1 / 2, u);
  State k3 = dt * model.StateSpaceModel(x + k2, u);
  return x + 1.0 / 6.0 * (k0 + 2 * k1 + 2 * k2 + k3);
}

}  // namespace dynamic
}  // namespace models
}  // namespace bark

#endif  // BARK_MODELS_DYNAMIC_INTEGRATION_HPP_


#include <iostream>
#include <boost/python.hpp>
#include "prx/utilities/general/constants.hpp"

using namespace boost::python;

void pyprx_utilities_general_constants()
{
	// Var("EPSILON", PRX_EPSILON);
	scope().attr("PRX_EPSILON") = PRX_EPSILON;
	scope().attr("PRX_INFINITY") = PRX_INFINITY;
	scope().attr("PRX_PI") = PRX_PI;

}


#pragma once
#ifndef CANNON_PHYSICS_RK4_INTEGRATOR_H
#define CANNON_PHYSICS_RK4_INTEGRATOR_H

/*!
 * \file cannon/physics/rk4_integrator.hpp
 * \brief File containing RK4Integrator class definition.
 */

#include <functional>

#include <Eigen/Dense>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>

using namespace Eigen;
using namespace boost::numeric::odeint;

namespace cannon {
  namespace physics {

    /*!
     * \brief Class representing a fourth-order Runge-Kutta integrator.
     */
    class RK4Integrator {
      public:
        using system_type = std::function<void(const VectorXd&, VectorXd&, double)>;

        RK4Integrator() = delete;

        /*!
         * \brief Constructor taking the system to integrate, state dimension,
         * and timestep for integration.
         */
        RK4Integrator(system_type system, unsigned int state_dim, double
            dt) : system_(system), state_dim_(state_dim), dt_(dt) {
          state_ = VectorXd::Zero(state_dim_);
        }

        /*!
         * \brief Perform a single integration step using the timestep of this
         * integrator.
         *
         * \returns The new state, post-integration.
         */
        const VectorXd& step();

        /*!
         * \brief Set the state of the system being integrated.
         *
         * \param state The state to set.
         */
        void set_state(const VectorXd& state);

        /*!
         * \brief Get the current state of the system being integrated.
         *
         * \returns The current state.
         */
        VectorXd get_state() const;

        /*!
         * \brief Set the current time for the system being integrated.
         *
         * \param t The time to set.
         */
        void set_time(double t);

        /*!
         * \brief Get the current time in the system being integrated.
         *
         * \returns The current time.
         */
        double get_time() const;

      private:
        //using stepper_type = runge_kutta4<VectorXd, double, VectorXd, double, vector_space_algebra>;
        using stepper_type = runge_kutta_dopri5<VectorXd, double, VectorXd, double, vector_space_algebra>;

        system_type system_; //!< The system to be integrated
        stepper_type stepper_; //!< Internal ODEInt stepper

        unsigned int state_dim_; //!< State dimension of the system being integrated
        VectorXd state_; //!< Current state of the system being integrated

        double dt_; //!< Integration timestep
        double t_ = 0.0; //!< Current time in the integrated system

    };

  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_RK4_INTEGRATOR_H */