#include "linear_operator.hpp"

#include <Eigen/Dense>
#include <array>
#include <cmath>
#include <set>
#include <unordered_map>

#include "bases.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "integration.hpp"

namespace Time {
using ::testing::DoubleEq;
using ::testing::ElementsAre;
using ::testing::Not;

template <typename LinearOperator, typename BasisIn, typename BasisOut>
void CheckMatrixTranspose(const SparseIndices<BasisIn> &indices_in,
                          const SparseIndices<BasisOut> &indices_out) {
  auto op = LinearOperator();
  Eigen::MatrixXd A = op.ToMatrix(indices_in, indices_out);
  Eigen::MatrixXd AT =
      Eigen::MatrixXd::Zero(indices_in.size(), indices_out.size());
  std::unordered_map<BasisIn *, int> indices_in_map;
  std::unordered_map<BasisOut *, int> indices_out_map;

  for (int i = 0; i < indices_in.size(); ++i) {
    assert(!indices_in_map.count(indices_in[i]));
    indices_in_map[indices_in[i]] = i;
  }
  for (int i = 0; i < indices_out.size(); ++i) {
    assert(!indices_out_map.count(indices_out[i]));
    indices_out_map[indices_out[i]] = i;
  }

  // Check A.
  for (int i = 0; i < indices_in.size(); ++i) {
    SparseVector<BasisIn> vec{{{indices_in[i], 1.0}}};
    auto op_vec = op.MatVec(vec);
    for (auto [fn, coeff] : op_vec) {
      EXPECT_THAT(coeff, Not(DoubleEq(0)));
    }
    auto op_vec_check = op.MatVec(vec, indices_out);
    for (auto [fn, coeff] : op_vec_check)
      ASSERT_DOUBLE_EQ(coeff, A(indices_out_map[fn], i));
  }

  // Create AT.
  for (int i = 0; i < indices_out.size(); ++i) {
    SparseVector<BasisOut> vec{{{indices_out[i], 1.0}}};
    auto op_vec = op.RMatVec(vec);
    for (auto [fn, coeff] : op_vec) AT(indices_in_map[fn], i) = coeff;

    auto op_vec_check = op.RMatVec(vec, indices_in);
    for (auto [fn, coeff] : op_vec_check)
      ASSERT_DOUBLE_EQ(coeff, AT(indices_in_map[fn], i));
  }

  // Check that they are the same.
  ASSERT_TRUE(A.transpose().isApprox(AT));
}

template <typename LinearOperator, typename BasisIn, typename BasisOut>
void CheckMatrixQuadrature(const SparseIndices<BasisIn> &indices_in,
                           bool deriv_in,
                           const SparseIndices<BasisOut> &indices_out,
                           bool deriv_out) {
  auto mat = LinearOperator().ToMatrix(indices_in, indices_out);
  for (int j = 0; j < indices_in.size(); ++j)
    for (int i = 0; i < indices_out.size(); ++i) {
      auto psi_j = indices_in[j];
      auto phi_i = indices_out[i];

      double ip = 0;
      auto eval = [psi_j, deriv_in, phi_i, deriv_out](double t) {
        return psi_j->Eval(t, deriv_in) * phi_i->Eval(t, deriv_out);
      };
      for (auto elem : phi_i->support())
        ip += Integrate(eval, *elem, /*degree*/ 3);

      ASSERT_NEAR(mat(i, j), ip, 1e-10);
    }
}

TEST(ContLinearScaling, ProlongateEval) {
  // Reset the persistent trees.
  Bases B;

  int ml = 62;
  // Now we check what happens when we also refine near the end points.
  B.three_point_tree.DeepRefine([ml](auto node) {
    return node->is_metaroot() ||
           (node->level() < ml &&
            (node->index() == 0 ||
             node->index() == (1LL << (node->level() - 1)) - 1));
  });

  auto Lambda = B.three_point_tree.NodesPerLevel();
  auto Delta = B.cont_lin_tree.NodesPerLevel();

  double n_t = 2048;
  for (int l = 0; l < ml; ++l) {
    for (int i = 0; i < Delta[l].size(); ++i) {
      // Prolongate a single hat function.
      SparseVector<ContLinearScalingFn> vec{{{Delta[l][i], 1.0}}};

      auto p_vec = Prolongate<ContLinearScalingFn>()(vec);
      // Check that the functions eval to the same thing.
      for (int x = 0; x < n_t; x++) {
        double t = x * 1.0 / n_t;
        double eval = 0;
        for (auto [phi, coeff] : p_vec) {
          eval += phi->Eval(t) * coeff;
        }
        ASSERT_DOUBLE_EQ(Delta[l][i]->Eval(t), eval);
      }
    }
  }
}

TEST(ContLinearScaling, CheckMatrixTransposes) {
  // Reset the persistent trees.
  Bases B;

  int ml = 7;

  B.three_point_tree.UniformRefine(ml);
  auto Lambda = B.three_point_tree.NodesPerLevel();
  auto Delta = B.cont_lin_tree.NodesPerLevel();

  for (int l = 1; l < ml; ++l) {
    CheckMatrixTranspose<Prolongate<ContLinearScalingFn>, ContLinearScalingFn,
                         ContLinearScalingFn>({Delta[l - 1]}, {Delta[l]});
    CheckMatrixTranspose<MassOperator<ContLinearScalingFn, ContLinearScalingFn>,
                         ContLinearScalingFn, ContLinearScalingFn>(
        {Delta[l - 1]}, {Delta[l - 1]});
    CheckMatrixTranspose<
        ZeroEvalOperator<ContLinearScalingFn, ContLinearScalingFn>,
        ContLinearScalingFn, ContLinearScalingFn>({Delta[l - 1]},
                                                  {Delta[l - 1]});
  }
}

TEST(ContLinearScaling, MatrixQuadrature) {
  // Reset the persistent trees.
  Bases B;

  int ml = 7;

  B.three_point_tree.UniformRefine(ml);
  auto Lambda = B.three_point_tree.NodesPerLevel();
  auto Delta = B.cont_lin_tree.NodesPerLevel();

  for (int l = 0; l < ml; ++l) {
    CheckMatrixQuadrature<
        MassOperator<ContLinearScalingFn, ContLinearScalingFn>,
        ContLinearScalingFn, ContLinearScalingFn>({Delta[l]}, false, {Delta[l]},
                                                  false);
  }
}

TEST(DiscLinearScaling, CheckMatrixTransposes) {
  // Reset the persistent trees.
  Bases B;

  int ml = 7;

  B.ortho_tree.UniformRefine(ml);
  auto Lambda = B.ortho_tree.NodesPerLevel();
  auto Delta = B.disc_lin_tree.NodesPerLevel();

  for (int l = 1; l < ml; ++l) {
    std::cout << "Prolongation" << std::endl;
    CheckMatrixTranspose<Prolongate<DiscLinearScalingFn>, DiscLinearScalingFn,
                         DiscLinearScalingFn>({Delta[l - 1]}, {Delta[l]});
    std::cout << "MassOperator" << std::endl;
    CheckMatrixTranspose<MassOperator<DiscLinearScalingFn, DiscLinearScalingFn>,
                         DiscLinearScalingFn, DiscLinearScalingFn>(
        {Delta[l - 1]}, {Delta[l - 1]});
    std::cout << "ZeroEvalOperator" << std::endl;
    CheckMatrixTranspose<
        ZeroEvalOperator<DiscLinearScalingFn, DiscLinearScalingFn>,
        DiscLinearScalingFn, DiscLinearScalingFn>({Delta[l - 1]},
                                                  {Delta[l - 1]});
  }
}

TEST(DiscLinearScaling, MatrixQuadrature) {
  // Reset the persistent trees.
  Bases B;

  int ml = 7;

  B.ortho_tree.UniformRefine(ml);
  auto Lambda = B.ortho_tree.NodesPerLevel();
  auto Delta = B.disc_lin_tree.NodesPerLevel();

  for (int l = 0; l < ml; ++l) {
    CheckMatrixQuadrature<
        MassOperator<DiscLinearScalingFn, DiscLinearScalingFn>,
        DiscLinearScalingFn, DiscLinearScalingFn>({Delta[l]}, false, {Delta[l]},
                                                  false);
  }
}

TEST(DiscContLinearScaling, CheckMatrixTransposes) {
  // Reset the persistent trees.
  Bases B;

  int ml = 7;

  B.three_point_tree.UniformRefine(ml);
  auto Lambda_3pt = B.three_point_tree.NodesPerLevel();
  auto Delta_3pt = B.cont_lin_tree.NodesPerLevel();

  B.ortho_tree.UniformRefine(ml);
  auto Lambda_ortho = B.ortho_tree.NodesPerLevel();
  auto Delta_ortho = B.disc_lin_tree.NodesPerLevel();

  for (int l = 1; l < ml; ++l) {
    CheckMatrixTranspose<MassOperator<ContLinearScalingFn, DiscLinearScalingFn>,
                         ContLinearScalingFn, DiscLinearScalingFn>(
        {Delta_3pt[l - 1]}, {Delta_ortho[l - 1]});
    CheckMatrixTranspose<MassOperator<DiscLinearScalingFn, ContLinearScalingFn>,
                         DiscLinearScalingFn, ContLinearScalingFn>(
        {Delta_ortho[l - 1]}, {Delta_3pt[l - 1]});
    CheckMatrixTranspose<
        ZeroEvalOperator<ContLinearScalingFn, DiscLinearScalingFn>,
        ContLinearScalingFn, DiscLinearScalingFn>({Delta_3pt[l - 1]},
                                                  {Delta_ortho[l - 1]});
    CheckMatrixTranspose<
        ZeroEvalOperator<DiscLinearScalingFn, ContLinearScalingFn>,
        DiscLinearScalingFn, ContLinearScalingFn>({Delta_ortho[l - 1]},
                                                  {Delta_3pt[l - 1]});
    CheckMatrixTranspose<
        TransportOperator<ContLinearScalingFn, DiscLinearScalingFn>,
        ContLinearScalingFn, DiscLinearScalingFn>({Delta_3pt[l - 1]},
                                                  {Delta_ortho[l - 1]});
  }
}

TEST(DiscContLinearScaling, ZeroEvalWorks) {
  // Reset the persistent trees.
  Bases B;

  int ml = 7;

  B.three_point_tree.UniformRefine(ml);
  auto Lambda_3pt = B.three_point_tree.NodesPerLevel();
  auto Delta_3pt = B.cont_lin_tree.NodesPerLevel();

  B.ortho_tree.UniformRefine(ml);
  auto Lambda_ortho = B.ortho_tree.NodesPerLevel();
  auto Delta_ortho = B.disc_lin_tree.NodesPerLevel();

  for (int l = 0; l < ml; ++l) {
    auto mat =
        ZeroEvalOperator<ContLinearScalingFn, DiscLinearScalingFn>().ToMatrix(
            {Delta_3pt[l]}, {Delta_ortho[l]});
    for (int j = 0; j < Delta_3pt[l].size(); ++j)
      for (int i = 0; i < Delta_ortho[l].size(); ++i)
        ASSERT_NEAR(mat(i, j),
                    Delta_3pt[l][j]->Eval(0.0) * Delta_ortho[l][i]->Eval(0.0),
                    1e-10);
  }
}
TEST(DiscContLinearScaling, MatrixQuadrature) {
  // Reset the persistent trees.
  Bases B;

  int ml = 7;

  B.three_point_tree.UniformRefine(ml);
  auto Lambda_3pt = B.three_point_tree.NodesPerLevel();
  auto Delta_3pt = B.cont_lin_tree.NodesPerLevel();

  B.ortho_tree.UniformRefine(ml);
  auto Lambda_ortho = B.ortho_tree.NodesPerLevel();
  auto Delta_ortho = B.disc_lin_tree.NodesPerLevel();

  for (int l = 0; l < ml; ++l) {
    CheckMatrixQuadrature<
        MassOperator<ContLinearScalingFn, DiscLinearScalingFn>,
        ContLinearScalingFn, DiscLinearScalingFn>({Delta_3pt[l]}, false,
                                                  {Delta_ortho[l]}, false);
    CheckMatrixQuadrature<
        MassOperator<DiscLinearScalingFn, ContLinearScalingFn>,
        DiscLinearScalingFn, ContLinearScalingFn>({Delta_ortho[l]}, false,
                                                  {Delta_3pt[l]}, false);
    bool matrices_are_transposes =
        MassOperator<ContLinearScalingFn, DiscLinearScalingFn>()
            .ToMatrix({Delta_3pt[l]}, {Delta_ortho[l]})
            .transpose()
            .isApprox(MassOperator<DiscLinearScalingFn, ContLinearScalingFn>()
                          .ToMatrix({Delta_ortho[l]}, {Delta_3pt[l]}));
    ASSERT_TRUE(matrices_are_transposes);

    CheckMatrixQuadrature<
        TransportOperator<ContLinearScalingFn, DiscLinearScalingFn>,
        ContLinearScalingFn, DiscLinearScalingFn>({Delta_3pt[l]}, true,
                                                  {Delta_ortho[l]}, false);
  }
}

}  // namespace Time


#include "timer.h"
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <iostream>

using numeric_t = double;

numeric_t flushCache() {
    const size_t N = 1000;
    const Eigen::MatrixXd A = Eigen::MatrixXd::Random(N, N);
    const Eigen::VectorXd x = Eigen::MatrixXd::Random(N, 1);

    return x.transpose() * A * x;
}

int main() {
    const numeric_t alpha = 1e-4;
    const size_t L = 1000;
    const size_t T = 1;
    const size_t N = 1000000;
    const numeric_t tau = 0.001;

    assert(tau < std::pow((L / static_cast<numeric_t>(N)), 2) / (2. * alpha));

    auto u0 = [](numeric_t x) {
        return std::sin(2. * M_PI * x / L);
    };
    Eigen::MatrixXd U = Eigen::MatrixXd::Zero(N, T / tau);

    Timer timer;

    for (size_t lap = 0; lap < 10; ++lap) {
        std::cout << "starting lap: " << lap << std::endl;
        timer.start();

        const Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N, 0, L);
        U.col(0) = x.unaryExpr(u0);

        for (size_t t = 0; t < ((T / tau) - 1); ++t) {
            for (size_t i = 0; i < N; ++i) {
                U(i, t + 1) = U(i, t) + (tau * alpha / (x(1) * x(1))) * (
                        U(((i - 1) + N) % N, t) - 2. * U(i, t) +
                        U((i + 1) % N, t)
                );
            }
        }

        timer.lap();

        std::cout << "flushing cache: " << flushCache() << std::endl;
    }

    std::cout << "Average runtime: " << timer.mean() << "s" << std::endl;
    std::cout << "Minimal runtime: " << timer.min() << "s" << std::endl;

    return 0;
}


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

#include "Framework/TestingFramework.hpp"

#include <algorithm>
#include <array>
#include <boost/optional/optional.hpp>
#include <cstddef>
#include <type_traits>

#include "ControlSystem/Averager.hpp"
#include "DataStructures/DataVector.hpp"
#include "Utilities/ConstantExpressions.hpp"

SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.Linear",
                  "[ControlSystem][Unit]") {
  double t = 0.0;
  const double dt = 0.1;
  constexpr size_t deriv_order = 2;
  const double final_time = 5.0;

  // test true and false `using_average_0th_deriv_of_q`
  Averager<deriv_order> averager_t(0.5, true);
  Averager<deriv_order> averager_f(0.5, false);

  CHECK(averager_t.using_average_0th_deriv_of_q());
  CHECK_FALSE(averager_f.using_average_0th_deriv_of_q());

  // define custom approx for second derivative checks
  Approx custom_approx = Approx::custom().epsilon(1.0e-12).scale(1.0);

  while (t < final_time) {
    // test using an analytic function f(t) = t
    const DataVector analytic_func = {t, 1.0, 0.0};

    // update exponential averager
    averager_t.update(t, {analytic_func[0]}, {0.1});
    averager_f.update(t, {analytic_func[0]}, {0.1});
    // compare values once averager has sufficient data
    if (averager_t(t)) {
      const auto result_t = averager_t(t).get();
      // check function value, which should agree with the effective time
      CHECK(approx(result_t[0][0]) == averager_t.average_time(t));
      // check first derivative
      CHECK(approx(result_t[1][0]) == analytic_func[1]);
      // check second derivative
      // The exponential averager uses finite differencing to approximate the
      // derivatives. The second derivative is only a first order approximation,
      // which is why we enforce a less stringent check here.
      CHECK(custom_approx(result_t[2][0]) == analytic_func[2]);
    }
    if (averager_f(t)) {
      const auto result_f = averager_f(t).get();
      // check function value, which should agree with the true time `t`
      CHECK(approx(result_f[0][0]) == t);
      // check first derivative
      CHECK(approx(result_f[1][0]) == analytic_func[1]);
      // check second derivative
      // The exponential averager uses finite differencing to approximate the
      // derivatives. The second derivative is only a first order approximation,
      // which is why we enforce a less stringent check here.
      CHECK(custom_approx(result_f[2][0]) == analytic_func[2]);
    }

    t += dt;
  }
}

SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.SemiAnalytic",
                  "[ControlSystem][Unit]") {
  double t = 0.0;
  constexpr size_t deriv_order = 2;
  const double final_time = 5.0;

  // The equations suggests that our exponential averager is equivalent
  // to the simple exponential averaging method of
  // F_avg(t) = \alpha*F(t) + (1 - \alpha)*F_avg(t_{-1}}
  // where \alpha = \tau_m / (W(t)*D) and D = 1.0 + \tau_m/tau_avg
  // However, the exponential averager that we are testing has time varying
  // weight, so we allow \alpha to be a function of time and update at
  // each timestep.

  // some vars for a simple exponential average to compare against
  std::array<DataVector, deriv_order + 1> avg_values{{{0.0}, {0.0}, {0.0}}};
  double avg_time = 0.0;

  // the measurement timescale (tau_m)
  const double tau_m = 0.1;
  const double avg_tscale_fac = 1.0;
  const double damping_time = 0.1;
  const double denom = 1.0 + tau_m / (avg_tscale_fac * damping_time);
  // the average weight, represents W(t) in the above comment
  double avg_weight = 0.0;

  Averager<deriv_order> averager(avg_tscale_fac, true);

  // define custom approx for second derivative checks
  Approx custom_approx = Approx::custom().epsilon(1.0e-12).scale(1.0);

  while (t < final_time) {
    // test using an analytic function f(t) = t**2
    const DataVector analytic_func = {square(t), 2.0 * t, 2.0};

    // update exponential averager
    averager.update(t, {analytic_func[0]}, {damping_time});
    // compare values once averager has sufficient data
    if (averager(t)) {
      // update the weight and \alpha
      avg_weight = (tau_m + avg_weight) / denom;
      const double alpha = tau_m / avg_weight / denom;

      // do simple average of time, analytic func and its analytic derivatives
      avg_time = alpha * t + (1.0 - alpha) * avg_time;
      avg_values[0] = alpha * analytic_func[0] + (1.0 - alpha) * avg_values[0];
      avg_values[1] = alpha * analytic_func[1] + (1.0 - alpha) * avg_values[1];
      avg_values[2] = alpha * analytic_func[2] + (1.0 - alpha) * avg_values[2];

      auto result = averager(t).get();

      // check that the effective times agree with the averaged time
      CHECK(approx(averager.average_time(t)) == avg_time);
      // check function value
      CHECK(approx(result[0][0]) == avg_values[0][0]);
      // check first derivative
      CHECK(approx(result[1][0]) == avg_values[1][0]);
      // check second derivative
      // Again, this check is slightly looser than the others due to
      // numerical differentiation (see comment in Averager.Linear test)
      CHECK(custom_approx(result[2][0]) == avg_values[2][0]);
    } else {
      avg_time = t;
      avg_values = {
          {{analytic_func[0]}, {analytic_func[1]}, {analytic_func[2]}}};
    }

    t += tau_m;
  }
}

SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.Functionality",
                  "[ControlSystem][Unit]") {
  double t = 0.0;
  const double dt = 0.1;
  constexpr size_t deriv_order = 2;

  Averager<deriv_order> averager(0.5, false);

  // test the validity of data functionality
  // data not valid yet
  CHECK_FALSE(static_cast<bool>(averager(t)));
  // first update
  averager.update(t, {t}, {0.1});
  // data not valid yet
  CHECK_FALSE(static_cast<bool>(averager(t)));
  t += dt;
  // second update
  averager.update(t, {t}, {0.1});
  // data not valid yet
  CHECK_FALSE(static_cast<bool>(averager(t)));
  t += dt;
  // third update
  averager.update(t, {t}, {0.1});
  // data should be valid now
  CHECK(static_cast<bool>(averager(t)));
  CHECK(averager(t).get()[0][0] == t);
  t += dt;
  // data should currently be invalid since there was no update at this new `t`
  CHECK_FALSE(static_cast<bool>(averager(t)));
  // last updated time should then be the time before this step
  CHECK(approx(averager.last_time_updated()) == (t - dt));

  // test the clear() function:
  // update averager again, so that data is valid
  averager.update(t, {t}, {0.1});
  CHECK(static_cast<bool>(averager(t)));
  // clear the averager, which should make the data no longer valid
  averager.clear();
  CHECK_FALSE(static_cast<bool>(averager(t)));
}

// [[OutputRegex, at or before the last time]]
SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.BadUpdateTwice",
                  "[ControlSystem][Unit]") {
  ERROR_TEST();
  Averager<2> averager(0.5, false);

  averager.update(0.5, {0.0}, {0.1});
  averager.update(0.5, {0.0}, {0.1});
}

// [[OutputRegex, at or before the last time]]
SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.BadUpdatePast",
                  "[ControlSystem][Unit]") {
  ERROR_TEST();
  Averager<2> averager(0.5, false);

  averager.update(0.5, {0.0}, {0.1});
  averager.update(0.3, {0.0}, {0.1});
}

// [[OutputRegex, The number of supplied timescales \(1\) does not match]]
SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.WrongSizeTimescales",
                  "[ControlSystem][Unit]") {
  ERROR_TEST();

  double t = 0.0;
  constexpr size_t deriv_order = 2;

  Averager<deriv_order> averager(1.0, false);
  averager.update(t, {{0.2, 0.3}}, {0.1});
}

// [[OutputRegex, The number of components in the raw_q provided \(2\) does]]
SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.WrongSizeQProvided",
                  "[ControlSystem][Unit]") {
  ERROR_TEST();

  double t = 0.0;
  constexpr size_t deriv_order = 2;

  Averager<deriv_order> averager(1.0, false);

  averager.update(t, {0.1}, {0.1});
  averager.update(t, {{0.2, 0.3}}, {0.1});
}

// [[OutputRegex, must be positive]]
SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.BadAvgTimescale",
                  "[ControlSystem][Unit]") {
  ERROR_TEST();
  Averager<2> averager(0.0, true);
}

// [[OutputRegex, The time history has not been updated yet]]
SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.BadCallToLastTimeUpdated",
                  "[ControlSystem][Unit]") {
  ERROR_TEST();
  Averager<2> averager(1.0, true);
  averager.last_time_updated();
}

// [[OutputRegex, Cannot return averaged values because the averager does not]]
SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.BadCallToAverageTime",
                  "[ControlSystem][Unit]") {
  ERROR_TEST();
  Averager<2> averager(1.0, true);
  averager.average_time(0.0);
}

SPECTRE_TEST_CASE("Unit.ControlSystem.Averager.TestMove",
                  "[ControlSystem][Unit]") {
  Averager<2> averager(0.25, false);
  static_assert(std::is_nothrow_move_constructible<Averager<2>>::value,
                "Averager is not nothrow move constructible");
  static_assert(std::is_nothrow_move_assignable<Averager<2>>::value,
                "Averager is not nothrow move assignable");
  // update with junk data
  averager.update(0.3, {0.1}, {0.1});
  averager.update(0.5, {0.2}, {0.1});
  averager.update(0.7, {0.4}, {0.1});
  averager.update(0.9, {0.6}, {0.1});
  // get correct values for comparison
  auto last_time = averager.last_time_updated();
  auto avg_time = averager.average_time(0.9);
  auto avg_q = averager.using_average_0th_deriv_of_q();
  auto avg_values = averager(0.9).get();
  // test move constructor
  auto new_averager(std::move(averager));
  // check moved values against stored values
  CHECK(last_time == new_averager.last_time_updated());
  CHECK(avg_time == new_averager.average_time(0.9));
  CHECK(avg_q == new_averager.using_average_0th_deriv_of_q());
  CHECK(avg_values[0][0] == new_averager(0.9).get()[0][0]);
  // test move assignment
  Averager<2> new_averager2(0.1, true);
  new_averager2 = std::move(new_averager);
  // check moved values against stored values
  CHECK(last_time == new_averager2.last_time_updated());
  CHECK(avg_time == new_averager2.average_time(0.9));
  CHECK(avg_q == new_averager2.using_average_0th_deriv_of_q());
  CHECK(avg_values[0][0] == new_averager2(0.9).get()[0][0]);
}


//
// ConfusionMatrix.hpp
// Represents a confusion matrix for multi-class evaluation
//

#ifndef FUMAROLE_LOCALIZATION_CONFUSIONMATRIX_HPP
#define FUMAROLE_LOCALIZATION_CONFUSIONMATRIX_HPP

#include <vector>
#include <string>
#include <tuple>
#include <iostream>

#ifdef __APPLE__
    #include <eigen3/Eigen/Eigen>
#else
    #include <Eigen/Eigen>
#endif

namespace Evaluation
{
    class ConfusionMatrix
    {
    public:
        /// Construct the matrix with the given class labels
        /// \param classLabels A list of class labels
        ConfusionMatrix(const std::vector<std::string>& classLabels);

        ~ConfusionMatrix();

        /// Add classification count for a classification output by the classifier
        /// \param predictedClass The name of the class that the classifier predicted
        /// \param actualClass The name of the class that was the ground truth class
        /// \param count The number of classifications made
        void AddClassifications(const std::string& predictedClass, const std::string& actualClass, int count = 1);

        /// Returns the accuracy of the classifier
        /// \return The accuracy score
        float GetAccuracy() const;

        /// Returns the precision of the classifier for the given class
        /// \param classLabel The name of the class
        /// \return The precision score
        float GetPrecision(const std::string& classLabel) const;

        /// Returns the recall of the classifier for the given class
        /// \param classLabel
        /// \return The recall score
        float GetRecall(const std::string& classLabel) const;

        /// Output confusion matrix as a CSV stream
        /// \param os output stream
        /// \param cm Confusion matrix
        /// \return A reference to the output stream
        friend std::ostream& operator<<(std::ostream& os, const ConfusionMatrix& cm);

        /// Append another confusion matrix scores
        /// \param other The other confusion matrix (dimensions must match)
        /// \return Return the reference to this matrix with the appended matrix data
        ConfusionMatrix& operator+=(const ConfusionMatrix& other);

    private:
        int IndexOfClass(const std::string& classLabel) const;

    private:
        std::vector<std::string> m_ClassLabels;
        Eigen::MatrixXi m_Matrix;
    };
}

#endif //FUMAROLE_LOCALIZATION_CONFUSIONMATRIX_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/algorithms/parse.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/extensions/gis/geographic/strategies/dms_parser.hpp>

#include <test_common/test_point.hpp>

using namespace boost::geometry;

template <typename P>
void test_2d(double c, bool use_strategy)
{
    // normal order, east=x, north=y
    P p;
    parse(p, std::string("1dE"), std::string("2N"));
    BOOST_CHECK_CLOSE( ((double) bg::get<0>(p)), (double) 1 * c, 1.0e-6);
    BOOST_CHECK_CLOSE( ((double) bg::get<1>(p)), (double) 2 * c, 1.0e-6);

    // reversed order, y,x -> should be interpreted correctly
    parse(p, std::string("1dN"), std::string("2E"));
    BOOST_CHECK_CLOSE( ((double) bg::get<0>(p)), (double) 2 * c, 1.0e-6);
    BOOST_CHECK_CLOSE( ((double) bg::get<1>(p)), (double) 1 * c, 1.0e-6);

    if (use_strategy)
    {
        // DUTCH system NOZW, only for degrees
        bg::strategy::dms_parser<false, 'N', 'O', 'Z', 'W'> strategy;
        parse(p, std::string("1dO"), std::string("2Z"), strategy);
        BOOST_CHECK_CLOSE( ((double) bg::get<0>(p)), (double) 1, 1.0e-6);
        BOOST_CHECK_CLOSE( ((double) bg::get<1>(p)), (double) -2, 1.0e-6);
    }

    // rest of DMS is checked in parse_dms
}

template <typename T, typename P>
void test_3d()
{
}

int test_main(int, char* [])
{
    //test_2d<point<int, 2, cs::geographic<radian> > >();
    //test_2d<point<float, 2, cs::geographic<radian> > >();

    test_2d<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >(1.0, true);
    test_2d<bg::model::point<double, 2, bg::cs::geographic<bg::radian> > >(bg::math::d2r, false);

    return 0;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/metaparse/repeated.hpp>
#include <boost/metaparse/sequence.hpp>
#include <boost/metaparse/lit_c.hpp>
#include <boost/metaparse/last_of.hpp>
#include <boost/metaparse/space.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/foldl_reject_incomplete_start_with_parser.hpp>
#include <boost/metaparse/one_of.hpp>
#include <boost/metaparse/get_result.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/transform.hpp>
#include <boost/metaparse/always.hpp>
#include <boost/metaparse/build_parser.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>

using boost::metaparse::sequence;
using boost::metaparse::lit_c;
using boost::metaparse::last_of;
using boost::metaparse::space;
using boost::metaparse::repeated;
using boost::metaparse::build_parser;
using boost::metaparse::int_;
using boost::metaparse::foldl_reject_incomplete_start_with_parser;
using boost::metaparse::get_result;
using boost::metaparse::one_of;
using boost::metaparse::token;
using boost::metaparse::entire_input;
using boost::metaparse::transform;
using boost::metaparse::always;

using boost::mpl::apply_wrap1;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::if_;
using boost::mpl::bool_;

/*
 * The grammar
 *
 * expression ::= plus_exp
 * plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*
 * prod_exp ::= value_exp ((mult_token | div_token) value_exp)*
 * value_exp ::= int_token | '_'
 */

typedef token<lit_c<'+'> > plus_token;
typedef token<lit_c<'-'> > minus_token;
typedef token<lit_c<'*'> > mult_token;
typedef token<lit_c<'/'> > div_token;

typedef token<int_> int_token;
typedef token<lit_c<'_'> > arg_token;

template <class T, char C>
struct is_c : bool_<T::type::value == C> {};

struct build_plus
{
  template <class A, class B>
  class _plus
  {
  public:
    typedef _plus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) + _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _minus
  {
  public:
    typedef _minus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) - _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '+'>::type,
      _plus<State, typename back<C>::type>,
      _minus<State, typename back<C>::type>
    >
  {};
};

struct build_mult
{
  template <class A, class B>
  class _mult
  {
  public:
    typedef _mult type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) * _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _div
  {
  public:
    typedef _div type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) / _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '*'>::type,
      _mult<State, typename back<C>::type>,
      _div<State, typename back<C>::type>
    >
  {};
};

struct build_value
{
  typedef build_value type;

  template <class V>
  struct apply
  {
    typedef apply type;

    template <class T>
    int operator()(T) const
    {
      return V::type::value;
    }
  };
};

struct arg
{
  typedef arg type;

  template <class T>
  T operator()(T t) const
  {
    return t;
  }
};

typedef
  one_of<transform<int_token, build_value>, always<arg_token, arg> >
  value_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<mult_token, div_token>, value_exp>,
    value_exp,
    build_mult
  >
  prod_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<plus_token, minus_token>, prod_exp>,
    prod_exp,
    build_plus
  >
  plus_exp;

typedef last_of<repeated<space>, plus_exp> expression;

typedef build_parser<entire_input<expression> > function_parser;

#if BOOST_METAPARSE_STD < 2011

template <class Exp>
struct lambda : apply_wrap1<function_parser, Exp> {};

using boost::metaparse::string;

lambda<string<'1','3'> >::type f1;
lambda<string<'2',' ','+',' ','3'> >::type f2;
lambda<string<'2',' ','*',' ','2'> >::type f3;
lambda<string<' ','1','+',' ','2','*','4','-','6','/','2'> >::type f4;
lambda<string<'2',' ','*',' ','_'> >::type f5;

#else

#ifdef LAMBDA
  #error LAMBDA already defined
#endif
#define LAMBDA(exp) apply_wrap1<function_parser, BOOST_METAPARSE_STRING(#exp)>::type

LAMBDA(13) f1;
LAMBDA(2 + 3) f2;
LAMBDA(2 * 2) f3;
LAMBDA( 1+ 2*4-6/2) f4;
LAMBDA(2 * _) f5;

#endif

int main()
{
  using std::cout;
  using std::endl;

  cout
    << f1(11) << endl
    << f2(11) << endl
    << f3(11) << endl
    << f4(11) << endl
    << f5(11) << endl
    << f5(1.1) << endl
    ;
}


#ifndef SHRS_HPP
#define SHRS_HPP

// std
#include <string>
#include <vector>
#include <utility>
#include <unordered_map>
#include <unordered_set>

// Eigen
#include <Eigen/Dense>

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <GridPoint.hpp>

namespace fsd {

using Eigen::Vector2d;
using std::vector;
using std::unordered_map;
using std::unordered_set;
using std::begin;
using std::end;
using std::endl;

/**
 * @brief The SHRS class spatial hashing radius search,
 * some data structure
 * for fast radius search in sparse pointcloud.
 * currently implemented only for 2D
 * and only for constant search_radius as
 * the cell_size is search_radius * 2.,
 * and no search circle rasterizing is implemented
 * yet but only getting the 1 - 4 cells which
 * this search circle can intersect
 * @tparam non-virtual dataset interface
 */
template<typename DatasetAdaptor>
class SHRS {
public:
    using key_t = typename DatasetAdaptor::key_t;
    using coords = GridPoint::coordinates;
    using hash_func_t = std::hash<key_t>;
    using cell_t = unordered_set<key_t, hash_func_t>;
    using search_map_t = unordered_map<key_t, cell_t, hash_func_t>;

    /**
      *@brief constructor; if adj_matrix is passed, its calculated
      */
    SHRS(DatasetAdaptor dataset_adaptor, double search_radius_,
         unordered_map<key_t, vector<key_t>> *adj_matrix = nullptr) :
        dataset_adaptor(dataset_adaptor), cell_size(search_radius_ * 2.),
        search_radius(search_radius_),
        search_radius_2(pow(search_radius_, 2.)) {
        // curently only search radius of cell_size
        // implemented
        cell_keys_to_search.reserve(4);
        search_map.max_load_factor(0.5);
        if(adj_matrix) {
            calculate_adj_matrix(*adj_matrix);
        }
    }

    SHRS &set_initial_cell_size(size_t value) {
        initial_cell_size = value;
        return *this;
    }

    /**
     * @brief find_neighbours find
     * all neigbours within set radius
     * @param key key of point
     * @return number of neighbours
     */
    size_t find_neighbours(key_t point_key,
                           vector<key_t> &neighbours) {
        neighbours.reserve(initial_neighbours_size);
        return on_find_neighbours(point_key, [&](key_t key) {
            neighbours.push_back(key);
        });
    }

    /**
     * @brief on_find_neighbours found neigbours
     * with callback to eventually avoid copy
     * @param point_key
     * @param func
     * @return
     */
    size_t on_find_neighbours(key_t point_key,
                              std::function<void(key_t)> func) {
        auto vec_pnt = dataset_adaptor.get_point(point_key);
        auto cell_coords =
                GridPoint::to_coordinates(vec_pnt, cell_size);
        auto cell_key = GridPoint::pack_key(cell_coords);
        if(search_map.find(cell_key) == end(search_map)) {
            // this point doesn't exist in search_map
            return 0;
        }
        size_t neighbours_found = 0;
        get_cell_keys_in_range(vec_pnt, cell_keys_to_search);
        // now search all cells ( maximum 4 cells are searched)
        for(auto cell_key_to_search : cell_keys_to_search) {
            if(search_map.find(cell_key_to_search) == end(search_map)) {
                continue;
            }
            auto &point_keys = search_map.at(cell_key_to_search);
            for(auto other_point_key : point_keys) {
                if(point_key == other_point_key) {
                    continue;
                }
                auto other_point =
                        dataset_adaptor.get_point(other_point_key);
                double distance2 = (vec_pnt - other_point).squaredNorm();
                if(distance2 < search_radius_2) {
                    func(other_point_key);
                    neighbours_found++;
                }
            }
        }
        return neighbours_found;
    }

    /**
     * @brief find_neighbours_vec find neigbours to a point
     * not in dataset
     * @param vec_pnt
     * @param neighbours
     * @return num of neigbours
     */
    size_t find_neighbours_vec(Vector2d const &vec_pnt,
                               vector<key_t> &neighbours) {
        neighbours.reserve(initial_neighbours_size);
        return on_find_neighbours_vec(vec_pnt, [&](key_t key) {
            neighbours.push_back(key);
        });
    }

    /**
     * @brief find_neighbours_vec find neigbours to a point
     * not in dataset
     * @param vec_pnt
     * @param neighbours
     * @return num of neigbours
     */
    size_t on_find_neighbours_vec(Vector2d const &vec_pnt,
                                  std::function<void(key_t)> func) {
        auto cell_coords =
                GridPoint::to_coordinates(vec_pnt, cell_size);
        size_t neighbours_found = 0;
        get_cell_keys_in_range(vec_pnt, cell_keys_to_search);
        // now search all cells ( maximum 4 cells are searched)
        for(auto cell_key_to_search : cell_keys_to_search) {
            if(search_map.find(cell_key_to_search) == end(search_map)) {
                continue;
            }
            auto &point_keys = search_map.at(cell_key_to_search);
            for(auto other_point_key : point_keys) {
                auto other_point =
                        dataset_adaptor.get_point(other_point_key);
                double distance2 = (vec_pnt - other_point).squaredNorm();
                if(distance2 < search_radius_2) {
                    func(other_point_key);
                    neighbours_found++;
                }
            }
        }
        return neighbours_found;
    }

    /**
     * @brief find_nn_vec finds like find_neighbours_vec, but
     * nearest neigbour, which is in search radius
     * @param vec_pnt
     * @param neighbours
     * @return
     */
    size_t find_nn_vec(Vector2d const &vec_pnt,
                       vector<key_t> &neighbours) {
        auto cell_coords =
                GridPoint::to_coordinates(vec_pnt, cell_size);
        size_t neighbours_found = 0;
        neighbours.resize(1);
        double min_distance = std::numeric_limits<double>::infinity();
        get_cell_keys_in_range(vec_pnt, cell_keys_to_search);
        // now search all cells ( maximum 4 cells are searched)
        for(auto cell_key_to_search : cell_keys_to_search) {
            if(search_map.find(cell_key_to_search) == end(search_map)) {
                continue;
            }
            auto &point_keys = search_map.at(cell_key_to_search);
            for(auto other_point_key : point_keys) {
                if(points_index.find(other_point_key) == points_index.end()){
                    continue;
                }
                auto other_point =
                        dataset_adaptor.get_point(other_point_key);
                double distance2 = (vec_pnt - other_point).squaredNorm();
                if(distance2 < search_radius_2) {
                    if(distance2 < min_distance) {
                        min_distance = distance2;
                        neighbours[0] = other_point_key;
                        neighbours_found = 1;
                    }
                }
            }
        }
        if(!neighbours_found) {
            neighbours.clear();
        }
        return neighbours_found;
    }

    /**
      *
      * @brief inserts all points and calaculates adj_matrix
      */
    void calculate_adj_matrix(unordered_map<key_t, vector<key_t>> &adj_matrix) {
        insert_all();
        auto num_points = points_index.size();
        adj_matrix.max_load_factor(0.5);
        adj_matrix.reserve(num_points);
        dataset_adaptor.for_all_keys([&](key_t key) {
            find_neighbours(key, adj_matrix[key]);
        });
    }

    /**
     * @brief insert_point inserts the point in the seach_map,
     * the point is referenced by key, this means in must be first
     * have inserted in the underlying datastructure provided by
     * DatasetAdaptor
     * @param point_key
     * @return true if inserted
     */
    void insert_point(key_t point_key) {
        auto vec_pnt = dataset_adaptor.get_point(point_key);
        auto cell_coords =
                GridPoint::to_coordinates(vec_pnt, cell_size);
        auto cell_key = GridPoint::pack_key(cell_coords);
        if(search_map.find(cell_key) == end(search_map)) {
            auto set_to_insert = cell_t(initial_cell_size);
            set_to_insert.max_load_factor(.5f);
            search_map.emplace(cell_key, set_to_insert);
        }
        search_map.at(cell_key).insert(point_key);
        points_index.insert(point_key);
    }

    /**
     * @brief insert_all inserts all points currently in the
     * dataset
     */
    void insert_all() {
        points_index.reserve(dataset_adaptor.size());
        dataset_adaptor.for_all_keys([&](key_t key) {
            insert_point(key);
        });
    }

    /**
     * @brief delete_point deleted point must be available
     * while deleting, it can be deleted only
     * aftewards from the underlying datastructure provided by
     * DatasetAdaptor
     * @param point_key
     * @return true if deleted
     */
    bool delete_point(key_t point_key) {
        if(!dataset_adaptor.find_point(point_key)) {
            return false;
        }
        auto vec_pnt = dataset_adaptor.get_point(point_key);
        auto cell_key = GridPoint::to_key(vec_pnt, cell_size);
        auto &points = search_map.at(cell_key);
        points.erase(point_key);
        points_index.erase(point_key);
        if(points.empty()) {
            search_map.erase(cell_key);
        }
        return true;
    }

    /**
     * @brief get_cell_keys_in_range gets all cells on which
     * a circle with diameter cell_size and the center vec_pnt
     * lays
     * @param vec_pnt the point which is the center
     * @param OUT keys of these cells
     *
     */
    void get_cell_keys_in_range(Vector2d const &vec_pnt,
                                vector<key_t> &keys) {
        keys.clear();
        auto cell_coords =
                GridPoint::to_coordinates(vec_pnt, cell_size);
        keys.push_back(GridPoint::pack_key(cell_coords));
        // offset of this point from center
        double const half_cell_size = cell_size / 2.;
        double row_d = vec_pnt.x();
        double column_d = vec_pnt.y();
        double row_offset = fmod(row_d, cell_size);
        double column_offset = fmod(column_d, cell_size);
        auto fourth_point = cell_coords;
        if(row_offset < 0) {
            row_offset += cell_size;
        }
        if(column_offset < 0) {
            column_offset += cell_size;
        }
        // to jump from -1 to 1 evtl, as
        // 0 is not used as row and column
        if(row_offset < half_cell_size) {
            if(cell_coords.row == 1) {
                fourth_point.row -= 2;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row - 2,
                                                          cell_coords.column)));
            } else {
                fourth_point.row -= 1;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row - 1,
                                                          cell_coords.column)));
            }
        } else if(row_offset > half_cell_size) {
            if(cell_coords.row == -1) {
                fourth_point.row += 2;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row + 2,
                                                          cell_coords.column)));
            } else {
                fourth_point.row += 1;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row + 1,
                                                          cell_coords.column)));
            }
        }
        if(column_offset < half_cell_size) {
            if(cell_coords.column == 1) {
                fourth_point.column -= 2;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row,
                                                          cell_coords.column - 2)));
            } else {
                fourth_point.column -= 1;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row,
                                                          cell_coords.column - 1)));
            }
        } else if(column_offset > half_cell_size) {
            if(cell_coords.column == -1) {
                fourth_point.column += 2;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row,
                                                          cell_coords.column + 2)));
            } else {
                fourth_point.column += 1;
                keys.push_back(GridPoint::pack_key(coords(cell_coords.row,
                                                          cell_coords.column + 1)));
            }
        }

        if(keys.size() == 3) {
            keys.push_back(GridPoint::pack_key(fourth_point));
        }
    }

    /**
     * @brief get_points_index
     * @return
     */
    cell_t const &get_points_index() const {
        return points_index;
    }

    void set_initial_neighbours_size(size_t value) {
        initial_neighbours_size = value;
    }

    /**
     * @brief clear clears the whole shrs map, but not the
     * undelying data structure
     */
    void clear() {
        points_index.clear();
        search_map.clear();
    }

    void delete_from_index(key_t idx) {
        points_index.erase(idx);
    }

protected:
    /**
     * @brief initial_cell_size initial size to reserve
     * on a new cell when constructing
     */
    size_t initial_cell_size = 7;

    /**
     * @brief initial_neighbours_size initial size to
     * allocate for neigbours
     */
    size_t initial_neighbours_size = 5;

    /**
     * @brief search_map
     */
    search_map_t search_map;

    /**
     * @brief dataset_adaptor
     */
    DatasetAdaptor dataset_adaptor;

    /**
     * @brief cell_size
     */
    double cell_size;

    /**
     * @brief search_radius
     */
    double search_radius;

    /**
     * @brief search_radius_2 squared_search_radius
     */
    double search_radius_2;

    /**
     * @brief cells_to_search cells to search on
     * radius query, memebr to avoid realocation
     */
    vector<key_t> cell_keys_to_search;

    /**
     * @brief points_index which points are saved
     * in the shrs grid
     */
    cell_t points_index;

};

} // end namespace fsd

#endif


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_MODULE CoMTest
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include "RBDyn/CoM.h"
#include "RBDyn/EulerIntegration.h"
#include "RBDyn/FA.h"
#include "RBDyn/FK.h"
#include "RBDyn/FV.h"
#include "RBDyn/MultiBody.h"
#include "RBDyn/MultiBodyConfig.h"
#include "RBDyn/MultiBodyGraph.h"

// arm
#include "XYZarm.h"

const double TOL = 0.000001;

std::tuple<rbd::MultiBody, rbd::MultiBodyConfig, rbd::MultiBodyGraph> makeXYZSarmRandomCoM(bool isFixed = true)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  typedef Eigen::Matrix<double, 1, 1> EScalar;

  MultiBodyGraph mbg;

  double mass = 1.;
  Matrix3d I = Matrix3d::Identity();
  Vector3d h = Vector3d::Random();

  RBInertiad rbi(mass, h, I);

  Body b0(RBInertiad((fabs(EScalar::Random()(0)) + 1e-8) * 10., h, I), "b0");
  Body b1(RBInertiad((fabs(EScalar::Random()(0)) + 1e-8) * 10., h, I), "b1");
  Body b2(RBInertiad((fabs(EScalar::Random()(0)) + 1e-8) * 10., h, I), "b2");
  Body b3(RBInertiad((fabs(EScalar::Random()(0)) + 1e-8) * 10., h, I), "b3");
  Body b4(RBInertiad((fabs(EScalar::Random()(0)) + 1e-8) * 10., h, I), "b4");

  mbg.addBody(b0);
  mbg.addBody(b1);
  mbg.addBody(b2);
  mbg.addBody(b3);
  mbg.addBody(b4);

  Joint j0(Joint::RevX, true, "j0");
  Joint j1(Joint::RevY, true, "j1");
  Joint j2(Joint::RevZ, true, "j2");
  Joint j3(Joint::Spherical, true, "j3");

  mbg.addJoint(j0);
  mbg.addJoint(j1);
  mbg.addJoint(j2);
  mbg.addJoint(j3);

  //                b4
  //             j3 | Spherical
  //  Root     j0   |   j1     j2
  //  ---- b0 ---- b1 ---- b2 ----b3
  //  Fixed    RevX   RevY    RevZ

  PTransformd to(Vector3d(0., 0.5, 0.));
  PTransformd from(Vector3d(0., -0.5, 0.));

  mbg.linkBodies("b0", to, "b1", from, "j0");
  mbg.linkBodies("b1", to, "b2", from, "j1");
  mbg.linkBodies("b2", to, "b3", from, "j2");
  mbg.linkBodies("b1", PTransformd(Vector3d(0.5, 0., 0.)), "b4", PTransformd(Vector3d(-0.5, 0., 0.)), "j3");

  MultiBody mb = mbg.makeMultiBody("b0", isFixed);

  MultiBodyConfig mbc(mb);
  mbc.zero(mb);

  return std::make_tuple(mb, mbc, mbg);
}

BOOST_AUTO_TEST_CASE(computeCoMTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBodyGraph mbg;

  double mass = 1.;
  Matrix3d I = Matrix3d::Identity();
  Vector3d h = Vector3d::Zero();
  Vector6d v = Vector6d::Zero();

  RBInertiad rbi(mass, h, I);

  Body b0(rbi, "b0");
  Body b1(rbi, "b1");
  Body b2(RBInertiad(2., h, I), "b2");
  Body b3(rbi, "b3");

  mbg.addBody(b0);
  mbg.addBody(b1);
  mbg.addBody(b2);
  mbg.addBody(b3);

  Joint j0(Joint::RevX, true, "j0");
  Joint j1(Joint::RevY, true, "j1");
  Joint j2(Joint::RevZ, true, "j2");

  mbg.addJoint(j0);
  mbg.addJoint(j1);
  mbg.addJoint(j2);

  //  Root     j0      j1     j2
  //  ---- b0 ---- b1 ---- b2 ----b3
  //  Fixed    RevX   RevY    RevZ

  PTransformd to(Vector3d(0., 0.5, 0.));
  PTransformd from(Vector3d(0., 0., 0.));

  mbg.linkBodies("b0", to, "b1", from, "j0");
  mbg.linkBodies("b1", to, "b2", from, "j1");
  mbg.linkBodies("b2", to, "b3", from, "j2");

  MultiBody mb = mbg.makeMultiBody("b0", true);
  MultiBodyConfig mbc(mb);
  mbc.zero(mb);

  mbc.q = {{}, {0.}, {0.}, {0.}};

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);
  forwardAcceleration(mb, mbc);

  Vector3d CoM = computeCoM(mb, mbc);
  Vector3d CoMV = computeCoMVelocity(mb, mbc);
  Vector3d CoMA = computeCoMAcceleration(mb, mbc);

  double handCoMX = 0.;
  double handCoMY = (0.5 * 1. + 1. * 2. + 1.5 * 1.) / 5.;
  double handCoMZ = 0.;
  BOOST_CHECK_EQUAL(CoM, Vector3d(handCoMX, handCoMY, handCoMZ));
  BOOST_CHECK_EQUAL(CoMV, Vector3d::Zero());
  BOOST_CHECK_EQUAL(CoMA, Vector3d::Zero());

  mbc.q = {{}, {cst::pi<double>() / 2.}, {0.}, {0.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);
  forwardAcceleration(mb, mbc);

  CoM = sComputeCoM(mb, mbc);
  CoMV = sComputeCoMVelocity(mb, mbc);
  CoMA = computeCoMAcceleration(mb, mbc);

  handCoMX = 0.;
  handCoMY = (0.5 * 1. + 0.5 * 2 + 0.5 * 1.) / 5.;
  handCoMZ = (0.5 * 2. + 1. * 1.) / 5.;

  BOOST_CHECK_EQUAL(CoM, Vector3d(handCoMX, handCoMY, handCoMZ));
  BOOST_CHECK_EQUAL(CoMV, Vector3d::Zero());
  BOOST_CHECK_EQUAL(CoMA, Vector3d::Zero());

  // test safe version
  mbc.bodyPosW = {I, I, I};
  BOOST_CHECK_THROW(sComputeCoM(mb, mbc), std::domain_error);
  BOOST_CHECK_THROW(sComputeCoMVelocity(mb, mbc), std::domain_error);
  BOOST_CHECK_THROW(sComputeCoMAcceleration(mb, mbc), std::domain_error);

  mbc.bodyPosW = {I, I, I, I};
  mbc.bodyVelB = {v, v, v};
  mbc.bodyAccB = {v, v, v};
  BOOST_CHECK_NO_THROW(sComputeCoM(mb, mbc));
  BOOST_CHECK_THROW(sComputeCoMVelocity(mb, mbc), std::domain_error);
  BOOST_CHECK_THROW(sComputeCoMAcceleration(mb, mbc), std::domain_error);
}

Eigen::Vector3d makeCoMDotFromStep(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc)
{
  using namespace Eigen;
  using namespace rbd;

  double step = 1e-8;

  MultiBodyConfig mbcTmp(mbc);

  Vector3d oC = computeCoM(mb, mbcTmp);
  eulerIntegration(mb, mbcTmp, step);
  forwardKinematics(mb, mbcTmp);
  forwardVelocity(mb, mbcTmp);
  Vector3d nC = computeCoM(mb, mbcTmp);

  return (nC - oC) / step;
}

Eigen::MatrixXd makeJDotFromStep(const rbd::MultiBody & mb,
                                 const rbd::MultiBodyConfig & mbc,
                                 rbd::CoMJacobianDummy & jac)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  double step = 1e-8;

  MultiBodyConfig mbcTmp(mbc);

  MatrixXd oJ = jac.jacobian(mb, mbcTmp);
  eulerIntegration(mb, mbcTmp, step);
  forwardKinematics(mb, mbcTmp);
  forwardVelocity(mb, mbcTmp);
  MatrixXd nJ = jac.jacobian(mb, mbcTmp);

  return (nJ - oJ) / step;
}

BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  sva::PTransformd I(sva::PTransformd::Identity());

  MultiBodyGraph mbg;
  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc, mbg) = makeXYZSarmRandomCoM();

  CoMJacobianDummy comJac(mb);

  /**
   *						Test jacobian with the com speed get by differentiation.
   *						Also test computeCoMVelocity and computeCoMAcceleration.
   */

  mbc.q = {{}, {0.}, {0.}, {0.}, {1., 0., 0., 0.}};
  mbc.alpha = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};
  mbc.alphaD = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};

  forwardKinematics(mb, mbc);

  auto testJacCoMVelAcc = [](const rbd::MultiBody & mb, rbd::MultiBodyConfig & mbc, rbd::CoMJacobianDummy & comJac) {
    forwardVelocity(mb, mbc);
    forwardAcceleration(mb, mbc);

    Vector3d CoMVel = computeCoMVelocity(mb, mbc);
    Vector3d CoMAcc = computeCoMAcceleration(mb, mbc);
    Vector3d CDot_diff = makeCoMDotFromStep(mb, mbc);
    MatrixXd CJac = comJac.jacobian(mb, mbc);
    MatrixXd CJacDot = comJac.jacobianDot(mb, mbc);

    BOOST_CHECK_EQUAL(CJac.rows(), 3);
    BOOST_CHECK_EQUAL(CJac.cols(), mb.nrDof());

    VectorXd alpha = dofToVector(mb, mbc.alpha);
    VectorXd alphaD = dofToVector(mb, mbc.alphaD);

    Vector3d CDot = CJac * alpha;
    Vector3d CDotDot = CJac * alphaD + CJacDot * alpha;

    BOOST_CHECK_SMALL((CDot_diff - CDot).norm(), TOL);
    BOOST_CHECK_SMALL((CDot_diff - CoMVel).norm(), TOL);
    BOOST_CHECK_SMALL((CDotDot - CoMAcc).norm(), TOL);
  };

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      mbc.alphaD[i][j] = 1.;

      testJacCoMVelAcc(mb, mbc, comJac);

      mbc.alpha[i][j] = 0.;
      mbc.alphaD[i][j] = 0.;
    }
  }

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      mbc.alphaD[i][j] = 1.;

      testJacCoMVelAcc(mb, mbc, comJac);
    }
  }
  mbc.alpha = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};
  mbc.alphaD = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};

  /**
   * Same test but with a different q.
   */

  Quaterniond q;
  q = AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitZ());
  mbc.q = {{}, {0.4}, {0.2}, {-0.1}, {q.w(), q.x(), q.y(), q.z()}};
  forwardKinematics(mb, mbc);

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      mbc.alphaD[i][j] = 1.;

      testJacCoMVelAcc(mb, mbc, comJac);

      mbc.alpha[i][j] = 0.;
      mbc.alphaD[i][j] = 0.;
    }
  }

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      mbc.alphaD[i][j] = 1.;

      testJacCoMVelAcc(mb, mbc, comJac);
    }
  }
  mbc.alphaD = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};

  // test safe functions

  mbc.bodyPosW = {I, I, I};
  BOOST_CHECK_THROW(comJac.sJacobian(mb, mbc), std::domain_error);
  mbc = MultiBodyConfig(mb);

  /**
   *						Test jacobianDot with the jacobianDot get by differentiation.
   */

  mbc.q = {{}, {0.}, {0.}, {0.}, {1., 0., 0., 0.}};
  mbc.alpha = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};
  mbc.alphaD = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};

  forwardKinematics(mb, mbc);

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);
      forwardAcceleration(mb, mbc);

      MatrixXd jacDot_diff = makeJDotFromStep(mb, mbc, comJac);
      MatrixXd jacDot = comJac.jacobianDot(mb, mbc);

      BOOST_CHECK_EQUAL(jacDot.rows(), 3);
      BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof());

      BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL);
      mbc.alpha[i][j] = 0.;
    }
  }

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);
      forwardAcceleration(mb, mbc);

      MatrixXd jacDot_diff = makeJDotFromStep(mb, mbc, comJac);
      MatrixXd jacDot = comJac.jacobianDot(mb, mbc);

      BOOST_CHECK_EQUAL(jacDot.rows(), 3);
      BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof());

      BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL);
      mbc.alpha[i][j] = 0.;
    }
  }
  mbc.alpha = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};

  /**
   * Same test but with a different q.
   */

  q = AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitZ());
  mbc.q = {{}, {0.4}, {0.2}, {-0.1}, {q.w(), q.x(), q.y(), q.z()}};
  forwardKinematics(mb, mbc);

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);
      forwardAcceleration(mb, mbc);

      MatrixXd jacDot_diff = makeJDotFromStep(mb, mbc, comJac);
      MatrixXd jacDot = comJac.jacobianDot(mb, mbc);

      BOOST_CHECK_EQUAL(jacDot.rows(), 3);
      BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof());

      BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL);
      mbc.alpha[i][j] = 0.;
    }
  }

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);
      forwardAcceleration(mb, mbc);

      MatrixXd jacDot_diff = makeJDotFromStep(mb, mbc, comJac);
      MatrixXd jacDot = comJac.jacobianDot(mb, mbc);

      BOOST_CHECK_EQUAL(jacDot.rows(), 3);
      BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof());

      BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL);
      mbc.alpha[i][j] = 0.;
    }
  }
  mbc.alpha = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}};

  // test safe functions

  mbc.bodyPosW = {I, I, I};
  BOOST_CHECK_THROW(comJac.sJacobianDot(mb, mbc), std::domain_error);
  mbc = MultiBodyConfig(mb);

  MotionVecd mv;
  mbc.bodyVelB = {mv, mv, mv};
  BOOST_CHECK_THROW(comJac.sJacobianDot(mb, mbc), std::domain_error);
  mbc = MultiBodyConfig(mb);

  mbc.bodyVelW = {mv, mv, mv};
  BOOST_CHECK_THROW(comJac.sJacobianDot(mb, mbc), std::domain_error);
  mbc = MultiBodyConfig(mb);
}

BOOST_AUTO_TEST_CASE(CoMJacobianTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBodyGraph mbg;
  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc, mbg) = makeXYZSarmRandomCoM();

  std::vector<double> weight(mb.nrBodies());
  for(std::size_t i = 0; i < weight.size(); ++i)
  {
    weight[i] = Eigen::Matrix<double, 1, 1>::Random()(0);
  }

  CoMJacobian comJac(mb, weight);
  CoMJacobianDummy comJacDummy(mb, weight);

  rbd::forwardKinematics(mb, mbc);
  rbd::forwardVelocity(mb, mbc);

  // test jacobian
  MatrixXd jacMat = comJac.jacobian(mb, mbc);
  MatrixXd jacDummyMat = comJacDummy.jacobian(mb, mbc);

  BOOST_CHECK_EQUAL(jacMat.rows(), 3);
  BOOST_CHECK_EQUAL(jacMat.cols(), mb.nrDof());

  BOOST_CHECK_SMALL((jacMat - jacDummyMat).norm(), TOL);

  // change configuration
  Quaterniond q;
  q = AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitZ());
  mbc.q = {{}, {0.4}, {0.2}, {-0.1}, {q.w(), q.x(), q.y(), q.z()}};
  forwardKinematics(mb, mbc);

  jacMat = comJac.jacobian(mb, mbc);
  jacDummyMat = comJacDummy.jacobian(mb, mbc);

  BOOST_CHECK_SMALL((jacMat - jacDummyMat).norm(), TOL);

  // Test jacobianDot

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      MatrixXd jacDotMat = comJac.jacobianDot(mb, mbc);
      MatrixXd jacDotDummyMat = comJacDummy.jacobianDot(mb, mbc);

      BOOST_CHECK_EQUAL(jacDotMat.rows(), 3);
      BOOST_CHECK_EQUAL(jacDotMat.cols(), mb.nrDof());

      BOOST_CHECK_SMALL((jacDotMat - jacDotDummyMat).norm(), TOL);
      mbc.alpha[i][j] = 0.;
    }
  }

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      MatrixXd jacDotMat = comJac.jacobianDot(mb, mbc);
      MatrixXd jacDotDummyMat = comJacDummy.jacobianDot(mb, mbc);

      BOOST_CHECK_EQUAL(jacDotMat.rows(), 3);
      BOOST_CHECK_EQUAL(jacDotMat.cols(), mb.nrDof());

      BOOST_CHECK_SMALL((jacDotMat - jacDotDummyMat).norm(), TOL);
    }
  }

  // Test velocity and normal acceleration function
  for(int i = 0; i < 50; ++i)
  {
    Eigen::VectorXd q(mb.nrParams()), alpha(mb.nrDof());
    q.setRandom();
    alpha.setRandom();
    // normalize free flyier and spherical joint
    q.head<4>().normalize();
    q.segment(mb.jointPosInParam(mb.jointIndexByName("j3")), 4).normalize();
    rbd::vectorToParam(q, mbc.q);
    rbd::vectorToParam(alpha, mbc.alpha);
    forwardKinematics(mb, mbc);
    forwardVelocity(mb, mbc);
    // calcul the normal acceleration since alphaD is zero
    forwardAcceleration(mb, mbc);

    // test com velocity
    const MatrixXd & comJacMat = comJac.jacobian(mb, mbc);
    Vector3d velFromJac = comJacMat * alpha;
    Vector3d velFromMbc = comJac.velocity(mb, mbc);

    BOOST_CHECK_SMALL((velFromJac - velFromMbc).norm(), TOL);

    // test com normal acceleration
    const MatrixXd & comJacDotMat = comJac.jacobianDot(mb, mbc);
    Vector3d normalAccFromJac = comJacDotMat * alpha;
    Vector3d normalAccFromMbc1 = comJac.normalAcceleration(mb, mbc);
    Vector3d normalAccFromMbc2 = comJac.normalAcceleration(mb, mbc, mbc.bodyAccB);

    BOOST_CHECK_SMALL((normalAccFromJac - normalAccFromMbc1).norm(), TOL);
    BOOST_CHECK_SMALL((normalAccFromJac - normalAccFromMbc2).norm(), TOL);
  }

  // create a multibody with new inertial parameter to test updateInertialParameters
  std::tie(mb, mbc, mbg) = makeXYZSarmRandomCoM();

  MultiBodyGraph badMbg;
  MultiBody badMb;
  MultiBodyConfig badMbc;
  std::tie(badMb, badMbc, badMbg) = makeXYZarm();

  BOOST_CHECK_THROW(comJac.sUpdateInertialParameters(badMb), std::domain_error);
  BOOST_CHECK_NO_THROW(comJac.sUpdateInertialParameters(mb));
  CoMJacobianDummy comJacDummyUpdated(mb, weight);

  rbd::forwardKinematics(mb, mbc);
  rbd::forwardVelocity(mb, mbc);

  // test jacobian with updated model
  jacMat = comJac.jacobian(mb, mbc);
  jacDummyMat = comJacDummyUpdated.jacobian(mb, mbc);
  BOOST_CHECK_SMALL((jacMat - jacDummyMat).norm(), TOL);

  // test weight getter/setter
  std::vector<double> weight2 = comJac.weight();
  BOOST_CHECK_EQUAL_COLLECTIONS(weight.begin(), weight.end(), weight2.begin(), weight2.end());

  for(std::size_t i = 0; i < weight2.size(); ++i)
  {
    weight2[i] += 10.;
  }

  comJac.weight(mb, weight2);
  BOOST_CHECK_EQUAL_COLLECTIONS(weight2.begin(), weight2.end(), comJac.weight().begin(), comJac.weight().end());

  CoMJacobianDummy comJacDummyWeight2(mb, weight2);
  // test jacobian with new weight
  jacMat = comJac.jacobian(mb, mbc);
  jacDummyMat = comJacDummyWeight2.jacobian(mb, mbc);
  BOOST_CHECK_SMALL((jacMat - jacDummyMat).norm(), TOL);
}


// MIT License
//
// Copyright (c) 2018 Michal Siedlaczek
//
// 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.

/// \file
/// \author Michal Siedlaczek
/// \copyright MIT License

#pragma once

#include <algorithm>
#include <cmath>
#include <functional>
#include <numeric>
#include <vector>

#include <boost/math/distributions/gamma.hpp>

namespace taily {

struct Feature_Statistics {
    static constexpr std::size_t struct_size = 2 * sizeof(double) + sizeof(int);
    double expected_value;
    double variance;
    std::int64_t frequency;

    [[nodiscard]] constexpr auto
    operator+(Feature_Statistics const& other) const -> Feature_Statistics
    {
        return Feature_Statistics{expected_value + other.expected_value,
                                  variance + other.variance,
                                  frequency + other.frequency};
    }

    auto to_stream(std::ostream& os) const -> std::ostream&
    {
        os.write(reinterpret_cast<const char*>(&expected_value), sizeof(expected_value));
        os.write(reinterpret_cast<const char*>(&variance), sizeof(variance));
        os.write(reinterpret_cast<const char*>(&frequency), sizeof(frequency));
        return os;
    }

    [[nodiscard]] static auto from_stream(std::istream& is) -> Feature_Statistics
    {
        Feature_Statistics stats;
        is.read(reinterpret_cast<char*>(&stats.expected_value), sizeof(stats.expected_value));
        is.read(reinterpret_cast<char*>(&stats.variance), sizeof(stats.variance));
        is.read(reinterpret_cast<char*>(&stats.frequency), sizeof(stats.frequency));
        return stats;
    }

    template<typename Feature_Range>
    [[nodiscard]] static constexpr auto
    from_features(Feature_Range const& features) -> Feature_Statistics
    {
        return from_features(std::begin(features), std::end(features));
    }

    template<typename Forward_Iterator>
    [[nodiscard]] static constexpr auto
    from_features(Forward_Iterator first, Forward_Iterator last) -> Feature_Statistics
    {
        if (first == last) return Feature_Statistics{0, 0, 0};
        std::int64_t count{0};
        auto accumulate_feature = [&count](double const& acc, double const& feature) {
            count += 1;
            return acc + feature;
        };
        double const sum = std::accumulate(first, last, double{0.0}, accumulate_feature);
        double const expected_value = sum / count;
        auto accumulate_squared = [&expected_value](double const& acc, double const& feature) {
            return acc + std::pow(expected_value - feature, 2.0);
        };
        double const variance = std::accumulate(first, last, double{0.0}, accumulate_squared)
            / count;
        return Feature_Statistics{expected_value, variance, count};
    }
};

struct Query_Statistics {
    std::vector<Feature_Statistics> term_stats;
    std::int64_t collection_size;
};

/// Extimates the number of documents containing **any** of the terms
/// represented by `term_stats` in a collection of size `collection_size`.
[[nodiscard]] auto any(Query_Statistics const& stats) -> double
{
    const auto collection_size = stats.collection_size;
    const double any_product = std::accumulate(
        stats.term_stats.begin(),
        stats.term_stats.end(),
        1.0,
        [collection_size](const auto& acc, const Feature_Statistics& stats) {
            return acc * (1.0 - double(stats.frequency) / collection_size);
        });
    return collection_size * (1.0 - any_product);
}

/// Extimates the number of documents containing **all** of the terms
/// represented by `term_stats` in a collection of size `collection_size`.
[[nodiscard]] auto all(const Query_Statistics& stats) -> double
{
    double const any = taily::any(stats);
    if (any == 0.0) {
        return 0.0;
    }
    double const all_product = std::accumulate(
        stats.term_stats.begin(),
        stats.term_stats.end(),
        1.0,
        [any](auto const& acc, Feature_Statistics const& stats) {
            return acc * (stats.frequency / any);
        });
    return any * all_product;
}

/// Returns a gamma distribution fitted to `term_stats`.
[[nodiscard]] auto
fit_distribution(Feature_Statistics const& query_term_stats) -> boost::math::gamma_distribution<>
{
    const double k = std::pow(query_term_stats.expected_value, 2.0)
        / query_term_stats.variance;
    const double theta =
        query_term_stats.variance / query_term_stats.expected_value;
    return boost::math::gamma_distribution<>(k, theta);
}

/// Returns a gamma distribution fitted to a vector of term stats.
///
/// The term statictics are accumulated before the distribution is fitted.
[[nodiscard]] auto fit_distribution(std::vector<Feature_Statistics> const& term_stats)
    -> boost::math::gamma_distribution<>
{
    Feature_Statistics query_stats = std::accumulate(
        term_stats.begin(), term_stats.end(), Feature_Statistics{0, 0, 0});
    return fit_distribution(query_stats);
}

/// Estimates the global cutoff score for the entire collection.
[[nodiscard]] auto estimate_cutoff(Query_Statistics const& stats, int ntop) -> double
{
    auto const dist = fit_distribution(stats.term_stats);
    double const all = taily::all(stats);
    double const p_c = std::min(1.0, ntop / all);
    return boost::math::quantile(complement(dist, p_c));
}

/// Calculates the probability that a document in a shard given by `stats`
/// has a score higher than `cutoff`.
[[nodiscard]] auto calculate_cdf(double const cutoff, Query_Statistics const& stats) -> double
// [[expects: cutoff >= 0.0]]
{
    if (cutoff <= 0) {
        return 1.0;
    }
    Feature_Statistics query_stats = std::accumulate(
        stats.term_stats.begin(), stats.term_stats.end(), Feature_Statistics{0, 0, 0});
    if (query_stats.expected_value == 0 || query_stats.variance == 0) {
        return 0.0;
    }
    auto dist = fit_distribution(query_stats);
    return boost::math::cdf(complement(dist, cutoff));
}

/// Scores shards given by `shard_stats`.
///
/// \param global_stats Term statistics for the entire collection
/// \param shard_stats Term statistics for individual shards
/// \param ntop The parameter to Taily algorithm saying how many top results we
/// are shooting for
[[nodiscard]] auto score_shards(Query_Statistics const& global_stats,
                                std::vector<Query_Statistics> const& shard_stats,
                                int const ntop) -> std::vector<double>
{
    int const shard_count = shard_stats.size();

    std::vector<double> shard_all(shard_count);
    std::transform(std::begin(shard_stats),
                   std::end(shard_stats),
                   std::begin(shard_all),
                   [](auto const& shard_stats) { return taily::all(shard_stats); });

    double const global_all = all(global_stats);
    double const global_cutoff = estimate_cutoff(global_stats, ntop);

    std::vector<double> shard_coefs(shard_count);
    std::transform(std::begin(shard_stats),
                   std::end(shard_stats),
                   std::begin(shard_coefs),
                   std::bind(calculate_cdf, global_cutoff, std::placeholders::_1));

    std::transform(std::begin(shard_coefs),
                   std::end(shard_coefs),
                   std::begin(shard_all),
                   std::begin(shard_coefs),
                   std::multiplies<double>());

    double const normalization_factor = std::accumulate(
        std::begin(shard_coefs), std::end(shard_coefs), 0.0);

    std::vector<double> estimates(shard_count);
    auto normalize = [ntop, normalization_factor](auto const& element) {
        return normalization_factor > 0 ? element * ntop / normalization_factor : 0.0;
    };
    std::transform(
        std::begin(shard_coefs), std::end(shard_coefs), std::begin(estimates), normalize);
    return estimates;
}

}  // namespace taily


#include "include_and_types.cpp"
#include <set>
#include <boost/graph/detail/set_adaptor.hpp>

typedef graph_traits<Graph>::adjacency_iterator adj_iter;

template <class Root, class InComponent, class Num, class Set>
class TransitiveClosure{
private:
    //Graph
    Graph*g;

    //Property Map instances
    Root root;
    InComponent inComp;
    Num num;
    Set succ;

    int index;

    //Data Structure feeding Property Map instances
    std::vector<Vertex> rootVet;
    std::vector<bool> inCompVet;
    std::vector<std::set<Vertex>*> sets;
    std::vector<int> numVet;
    std::vector<std::set<Vertex>> vect;
    std::stack<Vertex> s;

    /**
     * This function is the body of the algorithm. It takes the vertex that the procedure is visiting in that time
     * and calculates its successors.
     * This is the C++ implementation of the pseudocode available on the Nuutila's paper
     * @param v, vertex visited
     */
    void tc(Vertex v){
        ++index;
        put(root, v, v);
        put(inComp, v, false);
        put(num, v, index);
        put(succ,v,&vect[v]);
        std::set<Vertex> roots;
        adj_iter ai, a_end;
        for(boost::tie(ai, a_end)= adjacent_vertices(v,*g); ai!=a_end;++ai){
            Vertex w=*ai;
            if(get(num,w)==0)//Checking if w is already visited
                tc(w);
            if(!get(inComp,get(root,w)))//Checking if w's root is already in a SCC
                put(root, v, get(num, get(root,w))<get(num,get(root,v)) ? get(root,w) : get(root,v));
            else
                //complexity logaritmic in size
                roots.insert(get(root,w));
        }
        std::set<Vertex>::iterator it;
        //complexity of set_union: 2*(size_set1+size_set2)-1
        //This loop inserts all the vertex in roots and their successors in v's root successors
        for(it=roots.begin();it!=roots.end();++it){
            Vertex r=*it;
            std::set<Vertex>* succRoot=get(succ, get(root,v));
            set_union(get(succ,r)->begin(),get(succ,r)->end(),succRoot->begin(), succRoot->end(),
                      std::inserter(*succRoot,succRoot->begin()));
            succRoot->insert(r);
        }

        if(get(root,v)==v){
            if(get(num,s.top())>=get(num,v)){
                get(succ,v)->insert(v);
                do{
                    Vertex w=s.top();
                    s.pop();
                    put(inComp,w,true);
                    if(v!=w){
                        //Pointer assignment, not a copy
                        std::set_union(get(succ,w)->begin(),get(succ,w)->end(),get(succ,v)->begin(),
                                       get(succ,v)->end(),std::inserter(*get(succ,v),get(succ,v)->begin()));
                        put(succ,w, get(succ,v));
                    }
                }while(get(num,s.top())>=get(num,v));
            }else{
                //if a vertex is root of a trivial SCC
                put(inComp,v,true);
            }
        }else{
            //If a vertex is not a root, it pushes his root into the stack, if it is not
            if(get(num,s.top())!=get(num, get(root,v)))
                s.push(get(root,v));
            get(succ,get(root,v))->insert(v);

        }
    }

    /**
     * This function launches the procedure and it performs the visit of all the graph's vertex
     */
    void transitive_closure_main(){
        vertex_iter vi,v_end;
        timer::auto_cpu_timer t;
        for(boost::tie(vi,v_end)=vertices(*g);vi!=v_end;++vi){
            Vertex v=*vi;
            if(get(num,v)==0){
                tc(v);
            }
        }
    };

public:
    /**
     * This is the constructor of the TransitiveClosure class. It takes the reference of the already allocated graph
     * done by the caller (main_transitive_closure.cpp).
     * The constructor initializes all the class attributes in proper way
     * @param graph pointer
     */
    TransitiveClosure(Graph*graph){
        g=graph;

        //Insertion and handling of a new vertex with lowest num possible in order to accomplish algorithm correctness
        Vertex starter=add_vertex(*g);
        int n=num_vertices(*g);
        numVet=*new std::vector<int>(n);
        num=make_iterator_property_map(numVet.begin(),get(vertex_index,*g));
        put(num, starter, -1);
        s.push(starter);
        remove_vertex(starter,*g);

        //Updating number of graph's vertices
        n--;

        //data structures allocation
        rootVet.resize(n);
        inCompVet.resize(n);
        sets.resize(n);
        vect.resize(n);

        //Properties instantiation
        root=make_iterator_property_map(rootVet.begin(), get(vertex_index,*g));
        inComp= make_iterator_property_map(inCompVet.begin(), get(vertex_index,*g));
        succ=make_iterator_property_map(sets.begin(),get(vertex_index,*g));

        //index needed
        index=0;
    }

    /**
     * This function is called by the main_transitive_closure callee. It launches the procedure and prints the
     * procedure's result when it is done.
     */
    void transitive_closure_scc(){
        //Execution of the procedure
        transitive_closure_main();

        //Printing our results
        std::cout << "Ours" << std::endl;
        IndexMap index = get(vertex_index,*g);
        vertex_iter it, it_end;
        for(boost::tie(it,it_end)=vertices(*g);it!=it_end;++it){
            Vertex v=*it;
            std::set<Vertex>* set=get(succ,get(root,v));
            std::cout << index[*it] << " --> ";
            std::set<Vertex>::iterator i;
            for(i=set->begin();i!=set->end();++i){
                std::cout << *i << " ";
            }
            std::cout << std::endl;
        }

    }
};


/*
ID: neo-white
LANG: C++
TASK: Kmeans_PlusPlus.cpp
*/

#include "Kmeans_PlusPlus.h"
#include "../Core/Core.h"
#include <boost/serialization/vector.hpp>
#include <boost/serialization/queue.hpp>
#include <boost/serialization/export.hpp>
#include <vector>
#include <queue>
#include <fstream>
#include <time.h>

//BOOST_CLASS_EXPORT(kmeans_plusplus::Kmeans_PlusPlus)
namespace kmeans_plusplus{

    //Flexible_vector *kmeans_plusplus(Flexible_vector *problem,int k)
    std::vector<int> Kmeans_PlusPlus::kmeans_plusplus(Flexible_vector *problem,int k)
    {
    //	fprintf(stderr,"In kmeans_Lloyd \n");
        int l = problem->prob.l;
        int n_clusters = k;
        int max_index = problem->prob.max_feature;

        //choose initial center uniformly at random
        std::vector<int> queue;
        queue.resize(l);
        std::fill(queue.begin(),queue.end(), 0);
        srand((unsigned)time(NULL));
        int index = (rand()%l);
        queue[index] = 1;
        int iter_counter = 0;
        std::cout<<"kmeans++:iteration "<< iter_counter<<". candidated center "<<index<<std::endl;

        //choose remaining centers
        int remainingInstances = l - 1;
        std::vector<double> distances;
        distances.resize(l);
        std::vector<double> cumProbs;
        cumProbs.resize(l);
        std::vector<double> weights;
        weights.resize(l);
        int ii,jj,kk;
        if(n_clusters > 1)
        {
            //proceed with selecting the rest
            //distances to the initial randomly chose center
            for(ii=0;ii<l;ii++)
                distances[ii] = euclidean_distance(problem->get_value_without_label(ii),problem->get_value_without_label(index));

            int center_candidate;
            //now choose the remaining cluster centers
            for(jj = 1; jj < n_clusters; jj++)
            {
                bool find = false;
                //distances converted to probabilities
                double sum_distances = 0;
                for(kk=0;kk<l;kk++)
                    sum_distances += distances[kk];
                for(kk=0;kk<l;kk++)
                    weights[kk] = distances[kk]/sum_distances;
                double sum_probs = 0;
                for(kk=0;kk<l;kk++)
                {
                    sum_probs += weights[kk];
                    cumProbs[kk] = sum_probs;
                }
                cumProbs[l-1] = 1.0;  //make sure there are no rounding issues
                //choose a random instance
                double prob = (double) rand()/RAND_MAX;  //random between 0 and 1
                for(ii=0;ii<l;ii++)
                {
                    if(prob < cumProbs[ii])
                    {
                        if(queue[ii] != 1)
                        {
                            queue[ii] = 1;
                            center_candidate = ii;
                            std::cout<<"kmeans++:iteration "<<iter_counter+1<<". candidated center "<<ii<<".prob "<<prob<<std::endl;
                            remainingInstances--;
                            find = true;
                            break;
                        }
                    }
                }
                iter_counter++;

                if(find == false)
                {
                    std::cout<<"kmeans++:iteration "<<iter_counter+1<<". No find center. prob "<<prob<<std::endl;
                    break;
                }

                if(remainingInstances == 0)
                    break;

                //prepares to choose the next cluster center.
                //check distances against the new cluster center to see if it is closer
                double newDist;
                for(ii = 0;ii < l;ii++)
                {
                    if(distances[ii] > 0)
                    {
                        newDist = euclidean_distance(problem->get_value_without_label(ii),problem->get_value_without_label(center_candidate));
                        if(newDist < distances[ii])
                        {
                            distances[ii] = newDist;
                        }
                    }
                }
            }
        }
        return queue;
    }

/*
    std::vector<int> Kmeans_Lloyd::init_random(size_t length, size_t random_seed)
    {
        int tmp;
        std::vector<int> queue;
        for(size_t i=0;i<length;i++)
            queue.push_back(0);

        srand((unsigned)time(NULL));
        for(size_t j=0;j<random_seed;j++)
        {
            while(1)
            {
                tmp = (rand()%length);
                if(queue[tmp] == 0)
                {
                    queue[tmp] = 1;
                    break;
                }
            }
        }
        return queue;
    }

    void Kmeans_Lloyd::finish(Flexible_vector *records, size_t feat_dim)
    {
        std::ofstream file;
        file.open("./result.txt");
        size_t n_records = records->prob.l;
        for(size_t i = 0; i < n_records; ++i)
        {
            std::vector<node> record;
            record.clear();
            record = records->get_value_without_label(i);
            double dis = (1<<16)-1;
            int who = -1;
            for(size_t k = 0; k < this->fvkl.prob.l; k++)
            {
                std::vector<node> centroid = this->fvkl.get_value_without_label(k);
                double tmp = euclidean_distance(record,centroid);

                if (tmp < dis){
                    who = k;
                    dis = tmp;
                }
            }
            file << who << std::endl;
        }
    }
*/
    double Kmeans_PlusPlus::euclidean_distance(std::vector<node> px,std::vector<node> py)
    {
    	double sum = 0;
    	double distances = 0;
        size_t ipx = 0;
        size_t ipy = 0;
        while((ipx <= (px.size()-1))||(ipy <= (py.size()-1)))
    	{
            if((ipx <= (px.size()-1))&&(ipy <= (py.size()-1)))
            {
    		    if(px[ipx].index == py[ipy].index)
    		    {
    		    	sum += (px[ipx].value-py[ipy].value)*(px[ipx].value-py[ipy].value);
    		    	++ipx;
    		    	++ipy;
    		    }
    		    else
    		    {
    		    	if(px[ipx].index > py[ipy].index)
                    {
                        sum += py[ipy].value*py[ipy].value;
    		    		++ipy;
                    }
    		    	else
                    {
                        sum += px[ipx].value*px[ipx].value;
    		    		++ipx;
                    }
    		    }
            }else if((ipx == px.size())&&(ipy <= (py.size()-1)))
            {
                sum += py[ipy].value*py[ipy].value;
    		    ++ipy;
            }else if((ipy == py.size())&&(ipx <= (px.size()-1)))
            {
                sum += px[ipx].value*px[ipx].value;
    	   		++ipx;
            }
    	}
        distances = sqrt(sum);
    	return distances;
    }
}


// ALGOLAB BGL Tutorial 3
// Flow example demonstrating
// - breadth first search (BFS) on the residual graph

// Compile and run with one of the following:
// g++ -std=c++11 -O2 bgl_residual_bfs.cpp -o bgl_residual_bfs ./bgl_residual_bfs
// g++ -std=c++11 -O2 -I path/to/boost_1_58_0 bgl_residual_bfs.cpp -o bgl_residual_bfs; ./bgl_residual_bfs

// Includes
// ========
// STL includes
#include <iostream>
#include <algorithm>
#include <vector>
#include <queue>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/tuple/tuple.hpp>

// BGL graph definitions
// =====================
// Graph Type with nested interior edge properties for Flow Algorithms
typedef  boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
  boost::property<boost::edge_capacity_t, long,
    boost::property<boost::edge_residual_capacity_t, long,
      boost::property<boost::edge_reverse_t, traits::edge_descriptor> > > >  graph;
// Interior Property Maps
typedef traits::vertex_descriptor vertex_desc;

typedef  boost::graph_traits<graph>::edge_descriptor      edge_desc;
typedef  boost::graph_traits<graph>::out_edge_iterator      out_edge_it;

// Custom Edge Adder Class, that holds the references
// to the graph, capacity map and reverse edge map
// ===================================================
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

// Main
void testcase() {
  // build graph
  int n, m, s, d;
  std::cin >> n >> m >> s >> d;
  graph G(2 * n);
  edge_adder adder(G);
  // auto rc_map = boost::get(boost::edge_residual_capacity, G);
  const vertex_desc v_source = boost::add_vertex(G);
  const vertex_desc v_sink = boost::add_vertex(G);

  for(int k = 0; k < m; k++) {
    int i, j;
    std::cin >> i >> j;
    adder.add_edge(2 * i + 1, 2 * j, 1); // from out_i to in_j
  }

  for(int k = 0; k < s; k++) {
    int i;
    std::cin >> i;
    adder.add_edge(v_source, 2 * i, 1); // from sink to in_u
  }

  for(int k = 0; k < d; k++) {
    int i;
    std::cin >> i;
    adder.add_edge(2 * i + 1, v_sink, 1); // from sink to in_u
  }

  for(int i = 0; i < n; i++)
    adder.add_edge(2 * i, 2 * i + 1, 1);

  // Find a min cut via maxflow
  int flow = boost::push_relabel_max_flow(G, v_source, v_sink);
  std::cout << flow << "\n";

}

int main() {
  std::ios_base::sync_with_stdio(false);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) testcase();
  return 0;
}


#ifndef PHD_PARTICLE_FILTER
#define PHD_PARTICLE_FILTER

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include "opencv2/ml.hpp"

#include "../likelihood/gaussian.hpp"
#include "../likelihood/multivariate_gaussian.hpp"
#include "../utils/image_generator.hpp"
#include "../utils/utils.hpp"
#include "hungarian.h"
#include "../em.hpp"

#include <time.h>
#include <float.h>
#include <vector>
#include <iostream>
#include <random>
#include <chrono>
#include <limits>
#include <algorithm>

using namespace cv;
using namespace std;
using namespace Eigen;

typedef struct particle {
    float x; /** current x coordinate */
    float y; /** current y coordinate */
    float width; /** current width coordinate */
    float height; /** current height coordinate */
    float scale; /** current velocity bounding box scale */
    float x_p; /** current x coordinate */
    float y_p; /** current y coordinate */
    float width_p; /** current width coordinate */
    float height_p; /** current height coordinate */
    float scale_p; /** current velocity bounding box scale */
} particle;

class PHDParticleFilter {
public:
    int n_particles;
    vector<particle> states;
    vector<double> weights;
   ~PHDParticleFilter();
    PHDParticleFilter(int _n_particles, bool verbose = false);
    PHDParticleFilter();
    void initialize(Mat& current_frame, vector<Rect> detections);
    void update(Mat& image, vector<Rect> detections);
    vector<MyTarget> estimate(Mat& image, bool draw = false);
    void predict();
    void resample();
    void draw_particles(Mat& image, Scalar color);
    bool is_initialized();

protected:
    mt19937 generator;
    vector<VectorXd> theta_x;
    vector<VectorXd> theta_y;
    bool initialized;
    normal_distribution<double> position_random_walk, velocity_random_walk, scale_random_walk;
    Size img_size;
    int max_height, max_width, min_height, min_width;
    int max_x, max_y, min_x, min_y;
    int particles_batch;
    vector<MyTarget> tracks;
    vector<Rect> birth_model;
    RNG rng;
    vector<int> labels, current_labels;
    bool verbose;
};

#endif


/* test_laplace.cpp
 *
 * Copyright Steven Watanabe 2014
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/laplace_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/laplace.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::laplace_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME laplace
#define BOOST_MATH_DISTRIBUTION boost::math::laplace
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME mean
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(-n, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME beta
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


/**
 * @file covariances.cpp
 * @brief How to recover covariances.
 * @author Michael Kaess
 * @version $Id: covariances.cpp 6377 2012-03-30 20:06:44Z kaess $
 *
 * Copyright (C) 2009-2013 Massachusetts Institute of Technology.
 * Michael Kaess, Hordur Johannsson, David Rosen,
 * Nicholas Carlevaris-Bianco and John. J. Leonard
 *
 * This file is part of iSAM.
 *
 * iSAM 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 2.1 of the License, or (at
 * your option) any later version.
 *
 * iSAM 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 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with iSAM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <iostream>
#include <stdio.h>

#include <Eigen/LU>

#include <isam/isam.h>

using namespace std;
using namespace isam;
using namespace Eigen;

Slam slam;

int main(int argc, const char* argv[]) {
  Pose2d origin;
  Noise noise = SqrtInformation(10. * eye(3));
  Pose2d_Node* pose_node_1 = new Pose2d_Node(); // create node
  slam.add_node(pose_node_1); // add it to the Slam graph
  Pose2d_Factor* prior = new Pose2d_Factor(pose_node_1, origin, noise); // create prior measurement, an factor
  slam.add_factor(prior); // add it to the Slam graph

  Pose2d_Node* pose_node_2 = new Pose2d_Node(); // create node
  slam.add_node(pose_node_2); // add it to the Slam graph

  Pose2d delta(1., 0., 0.);
  Pose2d_Pose2d_Factor* odo = new Pose2d_Pose2d_Factor(pose_node_1, pose_node_2, delta, noise);
  slam.add_factor(odo);

  slam.batch_optimization();

#if 0
  const Covariances& covariances = slam.covariances();
#else
  // operate on a copy (just an example, cloning is useful for running
  // covariance recovery in a separate thread)
  const Covariances& covariances = slam.covariances().clone();
#endif

  // recovering the full covariance matrix
  cout << "Full covariance matrix:" << endl;
  MatrixXd cov_full = covariances.marginal(slam.get_nodes());
  cout << cov_full << endl << endl;

  // sanity checking by inverting the information matrix, not using R
  SparseSystem Js = slam.jacobian();
  MatrixXd J(Js.num_cols(), Js.num_cols());
  for (int r=0; r<Js.num_cols(); r++) {
    for (int c=0; c<Js.num_cols(); c++) {
      J(r,c) = Js(r,c);
    }
  }
  MatrixXd H = J.transpose() * J;
  MatrixXd cov_full2 = H.inverse();
  cout << cov_full2 << endl;

  // recovering the block-diagonals only of the full covariance matrix
  cout << "Block-diagonals only:" << endl;
  Covariances::node_lists_t node_lists;
  list<Node*> nodes;
  nodes.push_back(pose_node_1);
  node_lists.push_back(nodes);
  nodes.clear();
  nodes.push_back(pose_node_2);
  node_lists.push_back(nodes);
  list<MatrixXd> cov_blocks = covariances.marginal(node_lists);
  int i = 1;
  for (list<MatrixXd>::iterator it = cov_blocks.begin(); it!=cov_blocks.end(); it++, i++) {
    cout << "block " << i << endl;
    cout << *it << endl;
  }

  // recovering individual entries, here the right block column
  cout << "Right block column:" << endl;
  Covariances::node_pair_list_t node_pair_list;
  node_pair_list.push_back(make_pair(pose_node_1, pose_node_2));
  node_pair_list.push_back(make_pair(pose_node_2, pose_node_2));
  list<MatrixXd> cov_entries = covariances.access(node_pair_list);
  for (list<MatrixXd>::iterator it = cov_entries.begin(); it!=cov_entries.end(); it++) {
    cout << *it << endl;
  }
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver 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.
 *
 *     PCMSolver 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 for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef ONELAYERERF_HPP
#define ONELAYERERF_HPP

#include <cmath>
#include <iosfwd>

#include "Config.hpp"

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

/*! \file OneLayerErf.hpp
 *  \class OneLayerErf
 *  \brief A erf dielectric profile
 *  \author Roberto Di Remigio
 *  \date 2015
 *  \note The parameter given from user input for width_ is divided by 6.0 in
 *  the constructor to keep consistency with \cite Frediani2004a
 */

class OneLayerErf
{
private:
    /// Dielectric constant on the left of the interface
    double epsilon1_;
    /// Dielectric constant one the right of the interface
    double epsilon2_;
    /// Width of the transition layer
    double width_;
    /// Center of the transition layer
    double center_;
    /*! Returns value of dielectric profile at given point
     *  \param[in] point where to evaluate the profile
     */
    double value(double point) const {
        double epsPlus = (epsilon1_ + epsilon2_) / 2.0;
        double epsMinus = (epsilon2_ - epsilon1_) / 2.0;
        double val = boost::math::erf((point - center_) / width_);
        return (epsPlus + epsMinus * val); //epsilon(r)
    }
    /*! Returns value of derivative of dielectric profile at given point
     *  \param[in] point where to evaluate the derivative
     */
    double derivative(double point) const {
        double factor = (epsilon2_ - epsilon1_) / (width_ * std::sqrt(M_PI));
        double t = (point - center_) / width_;
        double val = std::exp(-std::pow(t, 2));
        return (factor * val); //first derivative of epsilon(r)
    }
    std::ostream & printObject(std::ostream & os)
    {
        os << "Profile functional form: erf" << std::endl;
        os << "Permittivity inside  = " << epsilon1_ << std::endl;
        os << "Permittivity outside = " << epsilon2_ << std::endl;
        os << "Profile width        = " << width_    << " AU" << std::endl;
        os << "Profile center       = " << center_   << " AU";
        return os;
    }
public:
    OneLayerErf() {}
    OneLayerErf(double e1, double e2, double w, double c) :
        epsilon1_(e1), epsilon2_(e2), width_(w/6.0), center_(c) {}
    /*! Returns a tuple holding the permittivity and its derivative
     *  \param[in]   r evaluation point
     */
    pcm::tuple<double, double> operator()(const double r) const
    {
        return pcm::make_tuple(value(r), derivative(r));
    }
    double epsilon1() const { return epsilon1_; }
    double epsilon2() const { return epsilon2_; }
    double width() const { return width_; }
    double center() const { return center_; }
    friend std::ostream & operator<<(std::ostream & os, OneLayerErf & th) {
        return th.printObject(os);
    }
};

#endif // ONELAYERERF_HPP