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

#include <vector>
#include <iterator>
#include <limits>
#include <cmath>
#include <numeric>
#include <ciso646>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <trng/uniform_dist.hpp>
#include <trng/uniform01_dist.hpp>
#include <trng/special_functions.hpp>
#include <trng/lcg64_shift.hpp>
#include <trng/exponential_dist.hpp>
#include <trng/twosided_exponential_dist.hpp>
#include <trng/normal_dist.hpp>
#include <trng/truncated_normal_dist.hpp>
#include <trng/maxwell_dist.hpp>
#include <trng/cauchy_dist.hpp>
#include <trng/logistic_dist.hpp>
#include <trng/lognormal_dist.hpp>
#include <trng/pareto_dist.hpp>
#include <trng/powerlaw_dist.hpp>
#include <trng/tent_dist.hpp>
#include <trng/weibull_dist.hpp>
#include <trng/extreme_value_dist.hpp>
#include <trng/gamma_dist.hpp>
#include <trng/beta_dist.hpp>
#include <trng/chi_square_dist.hpp>
#include <trng/student_t_dist.hpp>
#include <trng/snedecor_f_dist.hpp>
#include <trng/rayleigh_dist.hpp>
#include <trng/bernoulli_dist.hpp>
#include <trng/uniform_int_dist.hpp>
#include <trng/binomial_dist.hpp>
#include <trng/negative_binomial_dist.hpp>
#include <trng/hypergeometric_dist.hpp>
#include <trng/geometric_dist.hpp>
#include <trng/poisson_dist.hpp>
#include <trng/discrete_dist.hpp>

using floats = boost::mpl::list<float, double, long double>;

// integration by Simpson rule
//
template<typename iter>
typename std::iterator_traits<iter>::value_type simpson_int(iter first, iter last) {
  typedef typename std::iterator_traits<iter>::value_type value_type;
  if (first == last)
    return value_type{0};
  value_type sum{0};
  auto n = std::distance(first, last);
  if (n % 2 == 0) {  // Pulcherima (3/8 rule) for even number of data points
    sum += (*first) * value_type(3) / value_type(8);
    ++first;
    sum += (*first) * value_type(9) / value_type(8);
    ++first;
    sum += (*first) * value_type(9) / value_type(8);
    ++first;
    sum += (*first) * value_type(3) / value_type(8);
    n -= 3;
  }
  if (n > 2) {
    sum *= value_type(3);
    sum += *first;
    ++first;
    --n;
    while (n > 0) {
      sum += value_type(4) * (*first);
      ++first;
      --n;
      if (n == 1)
        sum += (*first);
      else
        sum += value_type(2) * (*first);
      ++first;
      --n;
    }
    sum /= value_type(3);
  }
  return sum;
}

// p value of Chi-squared test
double chi_percentil(const std::vector<double> &p, const std::vector<int> &count) {
  using size_type = std::vector<double>::size_type;
  const int N{std::accumulate(count.begin(), count.end(), 0)};
  const size_type bins{p.size()};
  double c2{0};
  for (size_type i{0}; i < bins; ++i) {
    const double expected{N * p[i]};
    c2 += (count[i] - expected) * (count[i] - expected) / expected;
  }
  const double c2_p{trng::math::GammaQ(c2 / 2, static_cast<double>(bins - 1) / 2)};
  return c2_p;
}

template<typename dist>
bool continous_dist_test_integrate_pdf(const dist &d) {
  using result_type = typename dist::result_type;
  // const int samples{1024 * 16 + 1};
  const int samples(std::min(1024ll * 1024,
                             static_cast<long long>(std::round(
                                 1 / std::sqrt(std::numeric_limits<result_type>::epsilon())))));
  const result_type x_min{d.icdf(result_type(1) / result_type(100))};
  const result_type x_max{d.icdf(result_type(99) / result_type(100))};
  const result_type dx{(x_max - x_min) / samples};
  std::vector<result_type> y;
  for (int i{0}; i <= samples; ++i)
    y.push_back(d.pdf(x_min + i * dx));
  const result_type s{simpson_int(y.begin(), y.end()) * dx};
  const result_type tol{result_type(64) / result_type(samples) / result_type(samples)};
  return std::abs(s - result_type(98) / result_type(100)) < tol;
}

template<typename dist>
boost::test_tools::predicate_result continous_dist_test_icdf(const dist &d) {
  using result_type = typename dist::result_type;
  const int bins{1024 * 1024};
  const result_type dp{result_type(1) / result_type(bins)};
  for (int i{1}; i < bins; ++i) {
    const result_type p{i * dp};
    const result_type x{d.icdf(p)};
    const result_type y{d.cdf(x)};
    if (std::abs(y - p) > 256 * std::numeric_limits<result_type>::epsilon()) {
      boost::test_tools::predicate_result res(false);
      res.message() << "cdf(icdf(p)) != p  for  p = " << p << " with cdf(icdf(p)) = " << y
                    << " and |cdf(icdf(p)) - p| = " << std::abs(y - p);
      return res;
    }
  }
  return true;
}

// failure of the chi2 test does not nessiarily imply an error, may happen just by chance
template<typename dist>
bool continous_dist_test_chi2_test(dist &d) {
  using result_type = typename dist::result_type;
  const int bins{128};
  const result_type dp{result_type(1) / result_type(bins)};
  const int N{10000};
  std::vector<result_type> quantil;
  std::vector<int> count(bins, 0);
  for (int i{1}; i < bins; ++i)
    quantil.push_back(d.icdf(dp * i));
  trng::lcg64_shift R;
  for (int i{0}; i < N; ++i) {
    const result_type x{d(R)};
    int bin{0};
    while (bin < bins - 1 and x > quantil[bin])
      ++bin;
    ++count[bin];
  }
  const std::vector<double> p(bins, 1.0 / bins);
  const double c2_p{chi_percentil(p, count)};
  return 0.01 < c2_p and c2_p < 0.99;
}

template<typename dist>
boost::test_tools::predicate_result discrete_dist_test(dist &d) {
  int i{d.min()};
  double P{d.cdf(i)};
  while (P < 0.95) {
    double p{P};
    if (i > d.min())
      p -= d.cdf(i - 1);
    const double diff{p - d.pdf(i)};
    if (diff > 128 * std::numeric_limits<double>::epsilon()) {
      boost::test_tools::predicate_result res(false);
      res.message() << "insufficient accuracy, i = " << i << ", pdf(i) = " << d.pdf(i)
                    << ", cdf(i) - cdf(i-1) = " << p
                    << ", |cdf(i) - cdf(i-1) - pdf(i)| = " << diff;
      return res;
    }
    ++i;
    P = d.cdf(i);
  }
  return true;
}

// failure of the chi2 test does not nessiarily imply an error, may happen just by chance
template<typename dist>
bool discrete_dist_test_chi2_test(dist &d) {
  using result_type = typename dist::result_type;
  std::vector<double> p;
  {
    int i{d.min()};
    while (i <= d.max()) {
      p.push_back(d.pdf(i));
      double P{d.cdf(i)};
      if (P > 0.99)
        break;
      ++i;
    }
    if (i < d.max())
      p.push_back(1.0 - d.cdf(i));
  }
  const int bins{static_cast<int>(p.size())};
  const int N{10000};
  std::vector<int> count(bins, 0);
  trng::lcg64_shift R(100ull);
  for (int i{0}; i < N; ++i) {
    const result_type x{d(R) - d.min()};
    int bin{std::min(x, bins - 1)};
    ++count[bin];
  }
  // merge bins with very small count numbers
  while (p.size() > 2) {
    auto min = std::min_element(count.begin(), count.end());
    if (*min > 8)
      break;
    auto pos = min - count.begin();
    const double p_old{p[pos]};
    const int count_old{count[pos]};
    p.erase(p.begin() + pos);
    count.erase(count.begin() + pos);
    min = std::min_element(count.begin(), count.end());
    pos = min - count.begin();
    p[pos] += p_old;
    count[pos] += count_old;
  }
  const double c2_p{chi_percentil(p, count)};
  return 0.01 < c2_p and c2_p < 0.99;
}

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

BOOST_AUTO_TEST_SUITE(test_suite_distributions)

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

BOOST_AUTO_TEST_SUITE(test_suite_continous_distributions)

BOOST_AUTO_TEST_CASE_TEMPLATE(test_uniform_dist, T, floats) {
  trng::uniform_dist<T> d(T(2), T(5));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_uniform01_dist, T, floats) {
  trng::uniform01_dist<T> d;
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_exponential_dist, T, floats) {
  trng::exponential_dist<T> d(T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_twosided_exponential_dist, T, floats) {
  trng::twosided_exponential_dist<T> d(T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_normal_dist, T, floats) {
  trng::normal_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_truncated_normal_dist, T, floats) {
  trng::truncated_normal_dist<T> d(T(5), T(2), T(2), T(6));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_maxwell_dist, T, floats) {
  trng::maxwell_dist<T> d(T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_cauchy_dist, T, floats) {
  trng::cauchy_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_logistic_dist, T, floats) {
  trng::logistic_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_lognormal_dist, T, floats) {
  trng::lognormal_dist<T> d(T(1), T(1) / T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_pareto_dist, T, floats) {
  trng::pareto_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_powerlaw_dist, T, floats) {
  trng::powerlaw_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_tent_dist, T, floats) {
  trng::tent_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_weibull_dist, T, floats) {
  trng::weibull_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_extreme_value_dist, T, floats) {
  trng::extreme_value_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_gamma_dist, T, floats) {
  trng::gamma_dist<T> d(T(5), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_beta_dist, T, floats) {
  trng::beta_dist<T> d(T(3), T(2));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_chi_square_dist, T, floats) {
  trng::chi_square_dist<T> d(T(38));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_student_t_dist, T, floats) {
  trng::student_t_dist<T> d(T(10));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_snedecor_f_dist, T, floats) {
  trng::snedecor_f_dist<T> d(T(10), T(11));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_rayleigh_dist, T, floats) {
  trng::rayleigh_dist<T> d(T(10));
  BOOST_TEST(continous_dist_test_integrate_pdf(d));
  BOOST_TEST(continous_dist_test_icdf(d));
  BOOST_TEST(continous_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_SUITE_END()

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

BOOST_AUTO_TEST_SUITE(test_suite_discrete_distributions)

BOOST_AUTO_TEST_CASE(test_bernoulli_dist) {
  trng::bernoulli_dist<int> d(0.4);
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE(test_uniform_int_dist) {
  trng::uniform_int_dist d(8, 100);
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE(test_binomial_dist) {
  trng::binomial_dist d(0.4, 20);
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE(test_negative_binomial_dist) {
  trng::negative_binomial_dist d(0.4, 20);
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE(test_hypergeometric_dist) {
  trng::hypergeometric_dist d(10, 5, 5);
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE(test_geometric_dist) {
  trng::geometric_dist d(0.3);
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE(test_poisson_dist) {
  trng::poisson_dist d(0.3);
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_CASE(test_discrete_dist) {
  std::vector<int> p{1, 2, 3, 4, 5, 4, 3, 2, 1};
  trng::discrete_dist d(p.begin(), p.end());
  BOOST_TEST(discrete_dist_test(d));
  BOOST_TEST(discrete_dist_test_chi2_test(d));
}

BOOST_AUTO_TEST_SUITE_END()

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

BOOST_AUTO_TEST_SUITE_END()


#include <boost/geometry/algorithms/centroid.hpp>


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_DIAGONAL_SETUP_INCLUDE
#define MTL_DIAGONAL_SETUP_INCLUDE

#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

namespace mtl { namespace matrix {

/// Setup a matrix to a multiple of the unity matrix
/** Intended for sparse matrices but works also with dense matrices.
    If the value is 0 the matrix is only zeroed out, whereby
    a sparse matrix will be empty after this operation,
    i.e. the zeros on the diagonal are not explicitly stored.
    The diagonal in its generalized form is the set of entries with equal row and column
    index (since r6843, older revision considered it erroneous to store
    a non-zero scalar to a non-square matrix).
 **/
template <typename Matrix, typename Value>
inline void diagonal_setup(Matrix& matrix, const Value& value)
{
    using std::min;
    if (num_rows(matrix) == 0 || num_cols(matrix) == 0)
	return;

    set_to_zero(matrix);
    inserter<Matrix>      ins(matrix, 1);
    for (typename Collection<Matrix>::size_type i= 0, n= min(num_rows(matrix), num_cols(matrix)); i < n; ++i)
	ins[i][i] << value;
}

}} // namespace mtl::matrix

#endif // MTL_DIAGONAL_SETUP_INCLUDE


#define _USE_MATH_DEFINES
#include <memory>
#include <iostream>
#include <time.h>
#include <unistd.h>
#include <fstream>
#include "yaml-cpp/yaml.h"
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include <thread>
#include <Eigen/Dense>

#include "geometry_msgs/msg/pose_array.hpp"
#include "rclcpp/rclcpp.hpp"
// #include "nav_msgs/msg/odometry.hpp"
// #include "geometry_msgs/msg/twist.hpp"
// #include "geometry_msgs/msg/vector3.hpp"
#include "msgs_car/msg/controls.hpp"
#include "msgs_car/msg/states.hpp"

// interface.cpp
// solver.c
// common.h

/* Some convenient definitions. */
#define NX          ACADO_NX  /* Number of differential state variables.  */
#define NXA         ACADO_NXA /* Number of algebraic variables. */
#define NU          ACADO_NU  /* Number of control inputs. */
#define NOD         ACADO_NOD  /* Number of online data values. */
#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
#define N           ACADO_N   /* Number of intervals in the horizon. */
#define VERBOSE		0
#define pi M_PI

using namespace std;
using namespace Eigen;
using std::placeholders::_1;

extern "C"{
__thread ACADOvariables acadoVariables;
__thread ACADOworkspace acadoWorkspace;
}

class MinimalPublisher : public rclcpp::Node
{
	public:

	int num_goal, num_obs, num;
	float dt, x_init, y_init, v_init, theta_init, thetadot_init, total_time, speed, avg_time, avg_speed;
	float prev_v_send, prev_w_send, a_obs, b_obs, w0, w1, w2, w3, w4;

	int cnt = 1, loop = -1;
	ofstream outdata, outdata2;
	ArrayXXf lane;
	ArrayXXf x_obs_temp, y_obs_temp, vx_obs, vy_obs, warm, Gotit, x_g, y_g, meta_cost;
	ArrayXXf batch_x, batch_y, batch_v, batch_w, batch_time, batch_theta,
				batch_optimal, batch_dist, batch_a, batch_j, old, batch_res_obs;
	MinimalPublisher(): Node("minimal_publisher"), count_(0)
    {
		speed = 0.0;
		total_time = 0.0;
		avg_speed = 0.0;
		avg_time = 0.0;
		prev_v_send = 0.0;
		prev_w_send = 0.0;

		num_obs = 6;
		num = 100;

		YAML::Node map = YAML::LoadFile("src/mpc_car_acado/config.yaml");

        string setting = map["setting"].as<string>();

        num_goal = map["configuration"][setting]["goal"].as<float>();
        w0 = map["configuration"][setting]["weights"][0].as<float>();
        w1 = map["configuration"][setting]["weights"][1].as<float>();
        w2 = map["configuration"][setting]["weights"][2].as<float>();
        w3 = map["configuration"][setting]["weights"][3].as<float>();

		dt = 0.08;
		x_init = 0.0;
		y_init = 10.0;
		theta_init = 0.0;

		warm = ArrayXXf(num_goal, 1);
		Gotit = ArrayXXf(num_goal, 1);
		warm = 0;
		Gotit = 0;

		x_obs_temp = ArrayXXf(num_obs, 1);
        y_obs_temp = ArrayXXf(num_obs, 1);
        vx_obs = ArrayXXf(num_obs, 1);
        vy_obs = ArrayXXf(num_obs, 1);
		old = ArrayXXf(num_goal, 1);

		batch_dist = ArrayXXf(num_goal, 1);
		batch_optimal = ArrayXXf(num_goal, 1);
		batch_time = ArrayXXf(num_goal, 1);
		batch_x = ArrayXXf(num_goal, 100);
		batch_y = ArrayXXf(num_goal, 100);
		batch_v = ArrayXXf(num_goal, 100);
		batch_w = ArrayXXf(num_goal, 100);
		batch_theta = ArrayXXf(num_goal, 100);
		batch_a = ArrayXXf(num_goal, 100);
		batch_j = ArrayXXf(num_goal, 100);
		batch_res_obs = ArrayXXf(num_goal, 1);

		batch_time = 10000000000000;

        x_obs_temp = 0;
        y_obs_temp = 0;
		vx_obs = 0;
        vy_obs = 0.0;
		a_obs = 5.6;
		b_obs = 3.0;

		x_g = ArrayXXf(num_goal, 1);
        y_g = ArrayXXf(num_goal, 1);

		meta_cost = ArrayXXf(num_goal, 9);
        meta_cost = -1;

		for(int i = 0; i < num_goal; i++)
        {
            x_g(i) = map["configuration"][setting]["x_g"][i].as<float>();
            y_g(i) = map["configuration"][setting]["y_g"][i].as<float>();
            meta_cost(i, 0) = i+1;
        }

		old = x_g;
		subscription_ = this->create_subscription<msgs_car::msg::States>(
        "ego_vehicle_obs", 10, std::bind(&MinimalPublisher::topic_callback, this, _1));

        publisher_ = this->create_publisher<msgs_car::msg::Controls>("ego_vehicle_cmds", 10);
        timer_ = this->create_wall_timer(10ms, bind(&MinimalPublisher::timer_callback, this));

		outdata.open(map["configuration"][setting]["file"].as<string>());
		thread Thread[num_goal];
		for(int i = 0; i < num_goal; i++)
			Thread[i] = thread(&MinimalPublisher::optimize, this, i);
		for(int i = 0; i < num_goal; i++)
			Thread[i].detach();

	}
	private:
    void topic_callback(const msgs_car::msg::States::SharedPtr msg)
    {
		if(Gotit.maxCoeff() == 0)
		{
			x_init = msg->x[0];
			y_init = msg->y[0];
			v_init = sqrt(msg->vx[0] * msg->vx[0] + msg->vy[0] * msg->vy[0]);
			theta_init = msg->psi[0];
			thetadot_init = msg->psidot;

			x_obs_temp << msg->x[1], msg->x[2], msg->x[3], msg->x[4], msg->x[5], msg->x[6];
			y_obs_temp << msg->y[1], msg->y[2], msg->y[3], msg->y[4], msg->y[5], msg->y[6];
			vx_obs << msg->vx[1], msg->vx[2], msg->vx[3], msg->vx[4], msg->vx[5], msg->vx[6];
			vy_obs << msg->vy[1], msg->vy[2], msg->vy[3], msg->vy[4], msg->vy[5], msg->vy[6];
			RCLCPP_INFO(this->get_logger(),"Got feedback");
			Gotit = 1;

			if(loop == -1)
				prev_v_send = v_init;
		}
	}
	void get_ranks();
	void timer_callback();
	void optimize(int info);
	rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<msgs_car::msg::States>::SharedPtr subscription_;
    rclcpp::Publisher<msgs_car::msg::Controls>::SharedPtr publisher_;
    size_t count_;
};
void MinimalPublisher :: get_ranks()
{
    float v_cruise = 15.0;
    for(int i = 0; i < num_goal; i++)
    {
        meta_cost(i, 1) = (batch_v.row(i) - v_cruise).matrix().lpNorm<2>(); // 5
        meta_cost(i, 2) = batch_optimal.row(i).matrix().lpNorm<2>();        // 6
        meta_cost(i, 3) = (batch_y.row(i) -  (-10)).matrix().lpNorm<2>(); // 7
        meta_cost(i, 4) = (batch_v.row(i) - 24).matrix().lpNorm<2>();                       // 8
        meta_cost(i, 5) = -1;
        meta_cost(i, 6) = -1;
        meta_cost(i, 7) = -1;
        meta_cost(i, 8) = -1;
    }
    float inf = std::numeric_limits<float>::infinity();
    for(int i = 0; i < num_goal; i++)
    {
        float min0 = inf, min1 = inf, min2 = inf, min3 = inf;
        int index0 = -1, index1 = -1, index2 = -1, index3 = -1;
        for(int j = 0; j < num_goal; j++)
        {
            if(meta_cost(j, 1) < min0 && meta_cost(j, 5) < 0)
            {
                min0 = meta_cost(j, 1);
                index0 = j;
            }
            if(meta_cost(j, 2) < min1 && meta_cost(j, 6) < 0)
            {
                min1 = meta_cost(j, 2);
                index1 = j;
            }
            if(meta_cost(j, 3) < min2 && meta_cost(j, 7) < 0)
            {
                min2 = meta_cost(j, 3);
                index2 = j;
            }
            if(meta_cost(j, 4) < min3 && meta_cost(j, 8) < 0)
            {
                min3 = meta_cost(j, 4);
                index3 = j;
            }
        }
        meta_cost(index0, 5) = i+1; // cruise
        meta_cost(index1, 6) = i+1; // optimal
        meta_cost(index2, 7) = i+1; // rightmost lane
        meta_cost(index3, 8) = i+1; // max average velocity
    }
}

void MinimalPublisher :: timer_callback()
{
    auto message = msgs_car::msg::Controls();

	int done = 1;
	for(int i = 0; i < num_goal; i++)
	{
		if(warm(i) == 0)
		{
			done = 0;
			break;
		}

	}
	if(done)
	{
		loop++;
		if(loop > 0)
		{
			total_time +=  batch_time.maxCoeff();
			avg_time = total_time/loop;
		}
		// if(x_init > 5*cnt) // 5 for rightmost + max
		{
			// x_g << 100, 110, 120, 130, 140, 150, 160, 170, 180, 190;
			// x_g << 120, 120, 120, 120, 120, 120, 130, 130, 130, 130;
			x_g = old + x_init;
			cnt+=1;
		}

		// 50 5 1 for 11 goals hsrl
		// cruise 30,70 ngsim data2

		//ngsim2 hsrl 0,50,5,1
		int index = 0;
		get_ranks();
		float min = 100000000;
		for(int i = 0; i < num_goal; i++)
		{
			float cost = w0 * meta_cost(i, 5) + w1 * meta_cost(i, 6) + w2 * meta_cost(i, 7) + w3 * meta_cost(i, 8);
                                // cruise                  optimal                   rightlane             max avg velocity
            if( cost < min)
            {
                min = cost;
                index = i;
            }
		}

		message.w = batch_w.row(index).leftCols(3).mean();
		message.v = batch_v.row(index).leftCols(3).mean();
		message.index = index;
		message.goals = num_goal;

		outdata << x_init << " " << y_init << " " << theta_init << " " <<message.v << " " << message.w
                << " " << (message.v - prev_v_send)/dt << " " << (message.w - prev_w_send)/dt
                << " " << batch_time.maxCoeff() << " " << loop+1 << " " << index << endl;
		// outdata2 << batch_x << " " << batch_y << endl;
		prev_v_send = message.v;
		prev_w_send = message.w;
		speed += message.v;
		avg_speed = speed/(loop + 2);

		for(int i = 0; i < num_goal; i++)
		{
			for(int j = 0; j < num; j++)
			{
				geometry_msgs::msg::Pose pose;
				pose.position.x = batch_x(i, j);
				pose.position.y = batch_y(i, j);
				message.batch.poses.push_back(pose);
			}
		}
		publisher_->publish(message);
		done = 0;
		warm = 0;
	}
	// if(loop > 0)
	// {
	// 	RCLCPP_INFO(this->get_logger(),"Time taken = %f Average speed = %f", avg_time, avg_speed);
	// }

}

void MinimalPublisher :: optimize(int info)
{
	// float x_g = info -> x_g, y_g = info -> y_g, theta_g = info -> theta_g;
	int id = info, NUM_STEPS = 15+0*15, first = 0;
	if(1 - first)
		acado_initializeSolver();
	while(1)
	{
		if(!Gotit(id)){
			sleep(0.001);
			continue;
		}
		acado_timer t, t2;
		for (int i = 0; i < N * (1 - first); ++i)
		{
			acadoVariables.u[ i*NU + 0 ] = 0.0;
			acadoVariables.u[ i*NU + 1 ] = 0.0;
		}
		for (int i = 0; i < N * (1 - first); ++i)
		{
			acadoVariables.y[ i*NY + 0 ] = 0.0;
			acadoVariables.y[ i*NY + 1 ] = 0.0;

			acadoVariables.y[ i*NY + 2 ] = 0.0;
			acadoVariables.y[ i*NY + 3 ] = 0.0;
			acadoVariables.y[ i*NY + 4 ] = 0.0;
			acadoVariables.y[ i*NY + 5 ] = 0.0;
			acadoVariables.y[ i*NY + 6 ] = 0.0;
			acadoVariables.y[ NY*i + 7] = 0.0;
		}
		acadoVariables.yN[ 0 ] = x_g(id);
		acadoVariables.yN[ 1 ] = y_g(id);
		acadoVariables.yN[ 2 ] = 0.0;
		acadoVariables.yN[ 3 ] = 0.0;

		acadoVariables.x0[ 0 ] = x_init;
		acadoVariables.x0[ 1 ] = y_init;
		acadoVariables.x0[ 2 ] = theta_init;		// theta
		acadoVariables.x0[ 3 ] = v_init;			// v
		acadoVariables.x0[ 4 ] = thetadot_init;		// w

		for (int i = 0; i < (N + 1)*(1 - first); i++)
		{
			acadoVariables.x[ i*NX + 0 ] = acadoVariables.x0[ 0 ];	//x
			acadoVariables.x[ i*NX + 1 ] = acadoVariables.x0[ 1 ];	//y
			acadoVariables.x[ i*NX + 2 ] = acadoVariables.x0[ 2 ];	//theta
			acadoVariables.x[ i*NX + 3 ] = acadoVariables.x0[ 3 ];	//v
			acadoVariables.x[ i*NX + 4 ] = acadoVariables.x0[ 4 ];	//w
		}

		for (int i = 0; i < (N + 1); ++i)
		{
			acadoVariables.od[i * NOD + 0] = x_obs_temp(0) + vx_obs(0) * dt * i;
			acadoVariables.od[i * NOD + 1] = y_obs_temp(0) + vy_obs(0) * dt * i;
			acadoVariables.od[i * NOD + 2] = x_obs_temp(1) + vx_obs(1) * dt * i;
			acadoVariables.od[i * NOD + 3] = y_obs_temp(1) + vy_obs(1) * dt * i;
			acadoVariables.od[i * NOD + 4] = x_obs_temp(2) + vx_obs(2) * dt * i;
			acadoVariables.od[i * NOD + 5] = y_obs_temp(2) + vy_obs(2) * dt * i;
			acadoVariables.od[i * NOD + 6] = x_obs_temp(3) + vx_obs(3) * dt * i;
			acadoVariables.od[i * NOD + 7] = y_obs_temp(3) + vy_obs(3) * dt * i;
			acadoVariables.od[i * NOD + 8] = x_obs_temp(4) + vx_obs(4) * dt * i;
			acadoVariables.od[i * NOD + 9] = y_obs_temp(4) + vy_obs(4) * dt * i;
			acadoVariables.od[i * NOD + 10] = x_obs_temp(5) + vx_obs(5) * dt * i;
			acadoVariables.od[i * NOD + 11] = y_obs_temp(5) + vy_obs(5) * dt * i;
		}

		for (int i = 0; i < N * (1 - first); i++)
		{
			acadoVariables.W[NY*NY*i + (NY+1)*0] = 7*1e3;	// a
			acadoVariables.W[NY*NY*i + (NY+1)*1] = 7*1e6;	// j
			acadoVariables.W[NY*NY*i + (NY+1)*2] = 3*2.8*1e4;
			acadoVariables.W[NY*NY*i + (NY+1)*3] = 3*2.8*1e4;
			acadoVariables.W[NY*NY*i + (NY+1)*4] = 3*2.8*1e4;
			acadoVariables.W[NY*NY*i + (NY+1)*5] = 3*2.8*1e4;
			acadoVariables.W[NY*NY*i + (NY+1)*6] = 3*2.8*1e4;
			acadoVariables.W[NY*NY*i + (NY+1)*7] = 3*2.8*1e4;
		}

		acadoVariables.WN[(NYN+1)*0] = 1e9;		// x terminal
		acadoVariables.WN[(NYN+1)*1] = 1e9;		// y terminal
		acadoVariables.WN[(NYN+1)*2] = 1e9;		// theta terminal
		acadoVariables.WN[(NYN+1)*3] = 1e6;

		// acado_preparationStep();

		acado_tic( &t );
		for(int iter = 0; iter < NUM_STEPS; ++iter)
		{
			acado_preparationStep();
			acado_feedbackStep( );
			// if(NUM_STEPS == 1)
			// {
			// 	acado_shiftStates(2, 0, 0);
        	// 	acado_shiftControls( 0 );
			// }
		}
		real_t te = acado_toc( &t );
		batch_time(id) = te;

		for(int i = 0; i < N; i++)
		{
			batch_x(id, i) = acadoVariables.x[i*NX + 0];
			batch_y(id, i) = acadoVariables.x[i*NX + 1];

			batch_theta(id, i) = 0.0;
			if(abs(batch_theta(id, i)) > 13.0 * M_PI/180.0)
				batch_theta(id, i) = acadoVariables.x[i*NX + 2];
			batch_v(id, i) = acadoVariables.x[i*NX + 3];
			batch_w(id, i) = acadoVariables.x[i*NX + 4];

			batch_dist(id) += sqrt(pow((x_obs_temp - batch_x(id, i)), 2) + pow((y_obs_temp - batch_y(id, i)), 2)).matrix().lpNorm<2>();
			batch_res_obs(id) += 1/(pow((x_obs_temp - batch_x(id, i))/a_obs, 2) + pow((y_obs_temp - batch_y(id, i))/b_obs, 2) - 1.0).matrix().lpNorm<2>();
			batch_a(id, i) = acadoVariables.u[i*NU + 0];
			batch_j(id, i) = acadoVariables.u[i*NU + 1];
		}

		warm(id) = 1;
		Gotit(id) = 0;
		first = 0;
		batch_optimal(id) = acado_getKKT();
		NUM_STEPS = 6;
		// if(NUM_STEPS != 5)
		// 	cout << "Thread complete= "<< id << " KKT Tol= " << acado_getKKT() << endl;

	}
}

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_HASH_SHA3_HPP
#define CRYPTO3_HASH_SHA3_HPP

#include <nil/crypto3/hash/detail/sponge_construction.hpp>
#include <nil/crypto3/hash/detail/block_stream_processor.hpp>
#include <nil/crypto3/hash/detail/sha3/sha3_functions.hpp>
#include <nil/crypto3/hash/detail/sha3/sha3_policy.hpp>
#include <nil/crypto3/hash/detail/sha3/sha3_finalizer.hpp>
#include <nil/crypto3/hash/detail/sha3/sha3_padding.hpp>

#include <boost/endian/conversion.hpp>

namespace nil {
    namespace crypto3 {
        namespace hashes {
            template<std::size_t DigestBits = 512>
            class sha3_compressor {
            protected:
                typedef detail::sha3_functions<DigestBits> policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t state_bits = policy_type::state_bits;
                constexpr static const std::size_t state_words = policy_type::state_words;
                typedef typename policy_type::state_type state_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                static void process_block(state_type &state, const block_type &block) {
                    for (std::size_t i = 0; i != block_words; ++i)
                        state[i] ^= block[i];

                    for (std::size_t i = 0; i != state_words; ++i)
                        boost::endian::endian_reverse_inplace(state[i]);

                    policy_type::permute(state);

                    for (std::size_t i = 0; i != state_words; ++i)
                        boost::endian::endian_reverse_inplace(state[i]);
                }
            };

            /*!
             * @brief
             * @tparam DigestBits
             * @ingroup hashes
             */
            template<std::size_t DigestBits = 512>
            class sha3 {
                typedef detail::sha3_functions<DigestBits> policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                typedef typename policy_type::digest_type digest_type;

                constexpr static const std::size_t pkcs_id_size = policy_type::pkcs_id_size;
                constexpr static const std::size_t pkcs_id_bits = policy_type::pkcs_id_bits;
                typedef typename policy_type::pkcs_id_type pkcs_id_type;

                constexpr static const pkcs_id_type pkcs_id = policy_type::pkcs_id;

                struct construction {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t length_bits = policy_type::length_bits;
                        constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                    };

                    typedef sponge_construction<params_type, typename policy_type::iv_generator,
                                                sha3_compressor<DigestBits>, detail::sha3_padding<policy_type>,
                                                detail::sha3_finalizer<policy_type>>
                        type;
                };

                template<typename StateAccumulator, std::size_t ValueBits>
                struct stream_processor {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t value_bits = ValueBits;
                    };
                    typedef block_stream_processor<construction, StateAccumulator, params_type> type;
                };
            };
        }    // namespace hashes
    }        // namespace crypto3
}    // namespace nil

#endif


#include "Pnm.h"

#include <iostream>

#include <vector>
#include <string>
#include <cmath>

#include <Eigen/Sparse>

typedef Eigen::SparseMatrix<double, Eigen::RowMajor> Matrix;
typedef Matrix::InnerIterator MatrixIterator;
typedef Eigen::BiCGSTAB <Eigen::SparseMatrix<double>> BiCGSTAB;
typedef Eigen::LeastSquaresConjugateGradient <Eigen::SparseMatrix<double>>
        LeastSqCG;
typedef Eigen::SparseLU <Eigen::SparseMatrix<double>> SparseLU;

Pnm::Pnm(const std::map <std::string, std::variant<int, double, std::string>> &paramsPnm,
         std::shared_ptr <Netgrid> netgrid) :
        _paramsPnm(paramsPnm),
        _netgrid(netgrid),
        _dim(netgrid->_poresPores.size()),
        _matrix(_dim, _dim),
        _freeVector(new double[_dim], _dim),
        _pressures(new double[_dim], _dim) {}

void Pnm::printParamsPnm() {

    for (auto const &[name, param] : _paramsPnm) {
        std::cout << name << ": ";
        if (std::get_if<int>(&param))
            std::cout << std::get<int>(param) << std::endl;
        else if (std::get_if<double>(&param))
            std::cout << std::get<double>(param) << std::endl;
        else if (std::get_if<std::string>(&param))
            std::cout << std::get<std::string>(param) << std::endl;
    }

}

void Pnm::calcConductances(const std::vector<double> &densities,
                           const std::vector<double> &viscosities) {

    for (auto &[throat, pores] : _netgrid->_throatsPores) {

        auto &height = _netgrid->_throatsDepths[throat];
        auto &width = _netgrid->_throatsWidths[throat];
        auto &length = _netgrid->_throatsLs[throat];
        auto &density = densities[throat];
        auto &viscosity = viscosities[throat];
        // 3D case
        // double resistance = 12. * viscosity * length / height / height / height / width;
        // 2D case
        auto corrFactor = 1.12;
        double resistance = corrFactor * 12. * viscosity * length / width / width / width / height;

        // _conductances[throat] = density / resistance;
        _conductances[throat] = 1. / resistance;
    }

}

void Pnm::processThroats() {
    for (auto&[throat, pores] : _netgrid->_throatsPores) {
        auto &normals = _netgrid->_normalsThroatsPores[throat];
        for (uint32_t i = 0; i < pores.size(); i++)
            _matrixCoeffs[throat][pores[i]] = normals[i] * _conductances[throat];
    }
}

void Pnm::processPores() {

    for (auto &pore: _netgrid->_inletPores)
        _freeCoeffs[pore] = 0;
    for (auto &pore: _netgrid->_outletPores)
        _freeCoeffs[pore] = 0;
    for (auto &pore: _netgrid->_deadendPores)
        _freeCoeffs[pore] = 0;
    for (auto &pore: _netgrid->_nonboundPores)
        _freeCoeffs[pore] = 0;
}

void Pnm::processNewmanPores(std::map<uint32_t, double> &poresFlows) {
    // ToDo: make sure it is ok with flow sign
    for (auto &[pore, flow]: poresFlows)
        _freeCoeffs[pore] = flow;
}

void Pnm::processDirichPores(std::map<uint32_t, double> &poresPressures) {
    for (auto &[pore, pressure]: poresPressures)
        _freeCoeffs[pore] = pressure;
}

void Pnm::fillMatrix(std::map<uint32_t, double> &poresFlows,
                     std::map<uint32_t, double> &poresPressures,
                     const std::vector<double> &capillaryPressures) {

    for (int i = 0; i < _dim; ++i)
        for (MatrixIterator it(_matrix, i); it; ++it)
            it.valueRef() = 0;

    std::set <uint32_t> groupedPores;
    groupedPores.insert(_netgrid->_nonboundPores.begin(), _netgrid->_nonboundPores.end());
    groupedPores.insert(_netgrid->_deadendPores.begin(), _netgrid->_deadendPores.end());

    for (auto &pore: groupedPores) {
        _freeVector[pore] = _freeCoeffs[pore];
        auto &throats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < throats.size(); i++) {
            _freeVector[pore] += normals[i] * _conductances[throats[i]] *
                                 capillaryPressures[throats[i]];
            for (auto &poreCurr : _netgrid->_throatsPores[throats[i]])
                _matrix.coeffRef(pore, poreCurr) +=
                        normals[i] * _matrixCoeffs[throats[i]][poreCurr];
        }
    }

    for (auto &[pore, flow]: poresFlows) {
        _freeVector[pore] = _freeCoeffs[pore];
        auto &throats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < throats.size(); i++) {
            _freeVector[pore] += normals[i] * _conductances[throats[i]] *
                                 capillaryPressures[throats[i]];
            for (auto &poreCurr : _netgrid->_throatsPores[throats[i]])
                _matrix.coeffRef(pore, poreCurr) +=
                        normals[i] * _matrixCoeffs[throats[i]][poreCurr];
        }
    }

    for (auto &[pore, pressure]: poresPressures) {
        _freeVector[pore] = _freeCoeffs[pore];
        _matrix.coeffRef(pore, pore) = 1;
    }

}

void Pnm::calculatePress() {

    auto &itAccuracy = std::get<double>(_paramsPnm["it_accuracy"]);
    auto &solverMethod = std::get<std::string>(_paramsPnm["solver_method"]);

    if (solverMethod == "sparseLU") {

        SparseLU sparseLU;
        sparseLU.compute(_matrix);
        _pressures = sparseLU.solve(_freeVector);

    } else if (solverMethod == "biCGSTAB") {

        BiCGSTAB biCGSTAB;
        biCGSTAB.compute(_matrix);
        biCGSTAB.setTolerance(itAccuracy);
        _pressures = biCGSTAB.solveWithGuess(_freeVector, _pressures);

    } else if (solverMethod == "leastSqCG") {

        LeastSqCG leastSqCG;
        leastSqCG.compute(_matrix);
        leastSqCG.setTolerance(itAccuracy);
        _pressures = leastSqCG.solveWithGuess(_freeVector, _pressures);

    }

}

void Pnm::cfdProcedure(const std::vector<double> &densities,
                       const std::vector<double> &viscosities,
                       const std::vector<double> &capillaryPressures,
                       std::map<uint32_t, double> &poresFlows,
                       std::map<uint32_t, double> &poresPressures) {

    calcConductances(densities, viscosities);
    processThroats();
    processPores();
    processNewmanPores(poresFlows);
    processDirichPores(poresPressures);
    fillMatrix(poresFlows, poresPressures, capillaryPressures);
    // std::cout << _matrix << std::endl;

    calculatePress();

}

void Pnm::calcThroatsVolFlows(const std::vector<double> &capillaryPressures) {

    for (auto &[throat, conductance] : _conductances) {
        auto &pores = _netgrid->_throatsPores[throat];
        auto &normals = _netgrid->_normalsThroatsPores[throat];
        _throatsVolFlows[throat] = conductance * capillaryPressures[throat];
        for (uint32_t i = 0; i < pores.size(); i++)
            _throatsVolFlows[throat] -= normals[i] * conductance * _pressures[pores[i]];
    }

}

void Pnm::calcPoresFlowRates() {

    std::set <uint32_t> groupedPores;
    groupedPores.insert(_netgrid->_nonboundPores.begin(), _netgrid->_nonboundPores.end());
    groupedPores.insert(_netgrid->_deadendPores.begin(), _netgrid->_deadendPores.end());

    for (auto &pore: groupedPores) {
        auto &poresThroats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < poresThroats.size(); i++)
            _poresFlowRates[pore] += normals[i] * _throatsVolFlows[poresThroats[i]];
    }

    for (auto &pore: _netgrid->_outletPores) {
        auto &poresThroats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < poresThroats.size(); i++)
            _poresFlowRates[pore] += normals[i] * _throatsVolFlows[poresThroats[i]];
    }

    for (auto &pore: _netgrid->_inletPores) {
        auto &poresThroats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < poresThroats.size(); i++)
            _poresFlowRates[pore] += -normals[i] * _throatsVolFlows[poresThroats[i]];
    }

}

void Pnm::calcTotalFlowRate(const std::set <uint32_t> &pores) {

    _totFlowRate = 0;

    for (auto &pore: pores)
        _totFlowRate += _poresFlowRates[pore];
}

void Pnm::setPressures(Eigen::Ref <Eigen::VectorXd> pressures) {
    if (_pressures.data() != pressures.data())
        delete _pressures.data();
    new(&_pressures) Eigen::Map<Eigen::VectorXd>(pressures.data(),
                                                 pressures.size());
}

Eigen::Ref <Eigen::VectorXd> Pnm::getPressures() {
    return _pressures;
}


#include <Eigen/Core>
// #include "densecrf.h"
#include "densecrf_wrapper.h"
#include <cstring>
#include <iostream>

DenseCRFWrapper::DenseCRFWrapper(int npixels, int nlabels)
: m_npixels(npixels), m_nlabels(nlabels) {
	// m_crf = new DenseCRF(npixels, nlabels);
}

DenseCRFWrapper::~DenseCRFWrapper() {
	// delete m_crf;
}

int DenseCRFWrapper::npixels() { return m_npixels; }
int DenseCRFWrapper::nlabels() { return m_nlabels; }

// std::vector< std::vector<float> >  DenseCRFWrapper::add_pairwise_energy(float* features_ptr,
// 			 int nfeatures, char * path, int path_length) {
// 	// std::cout << path_ << std::endl;
// 	std::string path_2(path);

// 	std::cout << "we are in dense crf" << std::endl;

// 	return m_crf->addPairwiseEnergy(
// 		Eigen::Map<const Eigen::MatrixXf>(features_ptr, nfeatures, m_npixels),
// 		path_2, DIAG_KERNEL,NORMALIZE_SYMMETRIC);

// }

std::vector< std::vector<float> >  DenseCRFWrapper::construct_sparse(float* features_ptr,
			 int nfeatures) {
	MatrixXf features = Eigen::Map<const Eigen::MatrixXf>(features_ptr, nfeatures, m_npixels);
	return lattice_.init(features);
	// return lattice_.compute_sparse_matrix();
}

// void DenseCRFWrapper::set_unary_energy(float* unary_costs_ptr) {
// 	m_crf->setUnaryEnergy(
// 		Eigen::Map<const Eigen::MatrixXf>(
// 			unary_costs_ptr, m_nlabels, m_npixels)
// 	);
// }

// void DenseCRFWrapper::map(int n_iters, int* labels) {
// 	VectorXs labels_vec = m_crf->map(n_iters);
// 	for (int i = 0; i < m_npixels; i ++)
// 		labels[i] = labels_vec(i);
// }


/* This file is part of PyMesh. Copyright (c) 2015 by Qingnan Zhou */
#include "SelfIntersection.h"

#include <Core/Exception.h>
#include <Math/MatrixUtils.h>

#include <boost/function.hpp>
#include <boost/bind.hpp>

#include <CGAL/box_intersection_d.h>

using namespace PyMesh;

namespace SelfIntersectionHelper {
    class Box : public CGAL::Box_intersection_d::Box_d<Float,
          3, CGAL::Box_intersection_d::ID_NONE> {
        public:
            typedef CGAL::Box_intersection_d::Box_d<Float, 3,
                    CGAL::Box_intersection_d::ID_NONE> Base;
            typedef Float                   NT;
            typedef std::size_t             ID;

            Box() {}
            Box(bool complete) : Base(complete) {}
            Box(NT l[3], NT h[3]) : Base( l, h) {}
            Box( const CGAL::Bbox_3& b) : Base( b) {}
            ID  id() const { return m_id; }
            void set_id(ID id) { m_id = id; }
        private:
            ID m_id;
    };

    void handle_intersection_candidate(
            SelfIntersection* self, const Box& a, const Box& b) {
        self->handle_intersection_candidate(a.id(), b.id());
    }

    Vector2I get_opposite_edge(const Vector3I& f, size_t v) {
        if (f[0] == v) {
            return Vector2I(f[1], f[2]);
        } else if (f[1] == v) {
            return Vector2I(f[2], f[0]);
        } else if (f[2] == v) {
            return Vector2I(f[0], f[1]);
        } else {
            std::stringstream err_msg;
            err_msg << "Vertex " << v << " does not belong to triangle ("
                << f.transpose() << ")";
            throw RuntimeError(err_msg.str());
        }
    }

    size_t get_opposite_vertex(const Vector3I& f, const Vector2I& e) {
        if (f[0] != e[0] && f[0] != e[1]) return f[0];
        if (f[1] != e[0] && f[1] != e[1]) return f[1];
        if (f[2] != e[0] && f[2] != e[1]) return f[2];
        throw RuntimeError("Face must be topologically degnerated!");
    }

    std::vector<Box> get_triangle_bboxes(
            const SelfIntersection::Points& pts, const MatrixIr& faces) {
        const size_t num_faces = faces.rows();
        std::vector<Box> boxes;
        boxes.reserve(num_faces);
        for (size_t i=0; i<num_faces; i++) {
            const Vector3I f = faces.row(i);
            const std::vector<SelfIntersection::Point_3> corners{
                pts[f[0]], pts[f[1]], pts[f[2]]
            };
            if (CGAL::collinear(pts[f[0]], pts[f[1]], pts[f[2]])) {
                // Triangle is degenerated.
                continue;
            }
            boxes.emplace_back(CGAL::bbox_3(corners.begin(), corners.end()));
            boxes.back().set_id(i);
        }
        return boxes;
    }
}
using namespace SelfIntersectionHelper;

SelfIntersection::SelfIntersection(
        const MatrixFr& vertices, const MatrixIr& faces)
: m_faces(faces) {
    const size_t num_vertices = vertices.rows();
    const size_t dim = vertices.cols();
    const size_t vertex_per_face = faces.cols();

    if (dim != 3) {
        throw NotImplementedError(
                "Self intersection check only support 3D");
    }
    if (vertex_per_face != 3) {
        throw NotImplementedError(
                "Self intersection check only works with triangles");
    }

    m_points.resize(num_vertices);
    for (size_t i=0; i<num_vertices; i++) {
        m_points[i] = Point_3(
                vertices(i,0),
                vertices(i,1),
                vertices(i,2));
    }
}

void SelfIntersection::detect_self_intersection() {
    clear();
    std::vector<Box> boxes = get_triangle_bboxes(m_points, m_faces);
    boost::function<void(const Box& a, const Box& b)> cb =
        boost::bind(SelfIntersectionHelper::handle_intersection_candidate,
                this, _1, _2);
    CGAL::box_self_intersection_d(boxes.begin(), boxes.end(), cb);
}

void SelfIntersection::clear() {
    m_intersecting_pairs.clear();
}

void SelfIntersection::handle_intersection_candidate(
        size_t f_idx_1, size_t f_idx_2) {
    auto duplicated_vertices = topological_overlap(f_idx_1, f_idx_2);
    const Vector3I f1 = m_faces.row(f_idx_1);
    const Vector3I f2 = m_faces.row(f_idx_2);
    const Triangle_3 t1(m_points[f1[0]], m_points[f1[1]], m_points[f1[2]]);
    const Triangle_3 t2(m_points[f2[0]], m_points[f2[1]], m_points[f2[2]]);

    bool is_intersecting = false;
    const size_t num_duplicated_vertices = duplicated_vertices.size();
    switch (num_duplicated_vertices) {
        case 0:
            // triangles do not touch.
            {
                bool t1_degenerate = t1.is_degenerate();
                bool t2_degenerate = t2.is_degenerate();
                if (t1_degenerate || t2_degenerate) {
                    // Degenerated triangles are considered as
                    // self-intersecting.
                    is_intersecting = true;
                } else {
                    is_intersecting = CGAL::do_intersect(t1, t2);
                }
            }
            break;
        case 3:
            // duplicated face
            is_intersecting = true;
            break;
        case 1:
            {
                // touch at a vertex.
                size_t shared_vertex = duplicated_vertices[0];
                Vector2I opp_edge_1 = get_opposite_edge(f1, shared_vertex);
                Vector2I opp_edge_2 = get_opposite_edge(f2, shared_vertex);
                Segment_3 seg_1(m_points[opp_edge_1[0]], m_points[opp_edge_1[1]]);
                Segment_3 seg_2(m_points[opp_edge_2[0]], m_points[opp_edge_2[1]]);
                is_intersecting =
                    CGAL::do_intersect(t1, seg_2) ||
                    CGAL::do_intersect(t2, seg_1);
            }
            break;
        case 2:
            {
                // touch at an edge.
                Vector2I shared_edge(duplicated_vertices[0],
                        duplicated_vertices[1]);
                size_t v1 = get_opposite_vertex(f1, shared_edge);
                size_t v2 = get_opposite_vertex(f2, shared_edge);
                const auto& p1 = m_points[v1];
                const auto& p2 = m_points[v2];
                const auto& p3 = m_points[shared_edge[0]];
                const auto& p4 = m_points[shared_edge[1]];
                if (CGAL::coplanar(p1, p2, p3, p4)) {
                    if (CGAL::collinear(p3, p4, p1)) {
                        is_intersecting = true;
                    } else if (CGAL::collinear(p3, p4, p2)) {
                        is_intersecting = true;
                    } else {
                        switch (CGAL::coplanar_orientation(p3, p4, p1, p2)) {
                            case CGAL::POSITIVE:
                                is_intersecting = true;
                                break;
                            case CGAL::NEGATIVE:
                                is_intersecting = false;
                                break;
                            case CGAL::COLLINEAR:
                                throw RuntimeError(
                                        "Inconsistent CGAL predicate output");
                                break;
                        }
                    }
                } else {
                    is_intersecting = false;
                }
            }
            break;
        default:
            throw RuntimeError(
                    "Two triangles sharing more than 3 vertices? Something is very wrong");
    }

    if (is_intersecting) {
        m_intersecting_pairs.emplace_back(f_idx_1, f_idx_2);
    }
}

std::vector<size_t> SelfIntersection::topological_overlap(size_t id1, size_t id2) const {
    Vector3I f1 = m_faces.row(id1);
    Vector3I f2 = m_faces.row(id2);
    std::vector<size_t> duplicated_vertices;
    duplicated_vertices.reserve(3);
    for (size_t i=0; i<3; i++) {
        for (size_t j=0; j<3; j++) {
            if (f1[i] == f2[j]) {
                duplicated_vertices.push_back(f1[i]);
            }
        }
    }
    return duplicated_vertices;
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/round2even.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( remquo_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::integer_<A1> >
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE
      result_type operator() ( A0 a0,A0 a1,A1& a3
                              , typename std::enable_if< std::is_same<
                                     typename bd::as_integer_t<A0,signed>
                                    , A1>::value>* = 0
                             ) const BOOST_NOEXCEPT
    {
      result_type a2;
      boost::simd::remquo(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( remquo_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using quo_t = bd::as_integer_t<A0, signed>;
    using result_type = std::pair<A0,quo_t>;

    BOOST_FORCEINLINE result_type operator() ( A0 a0,A0 a1) const BOOST_NOEXCEPT
    {
      A0 first;
      quo_t second;
      boost::simd::remquo( a0, a1, first, second );
      return result_type(first, second);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( remquo_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<A0> >
                          , bd::generic_<bd::floating_<A0> >
                          , bd::generic_<bd::floating_<A0> >
                          , bd::generic_<bd::integer_ <A1> >
                          )
  {

    BOOST_FORCEINLINE void operator() ( A0 a0, A0 a1,A0& a2, A1& a3
                                      , typename std::enable_if< std::is_same<
                                        typename bd::as_integer_t<A0,signed>
                                      , A1>::value>* = 0
                                      ) const BOOST_NOEXCEPT
    {
      A0 const d = round2even(a0/a1);

#if defined(BOOST_SIMD_NO_INVALIDS)
      a2 = if_allbits_else(is_eqz(a1), a0-d*a1);
#else
      a2 = if_allbits_else(logical_or(is_invalid(a0), is_eqz(a1)), a0-d*a1);
#endif

      a3 = toint(d);
    }
  };
} } }

#endif


#include <algorithm>
#include <array>
#include <cmath>

#include <boost/math/constants/constants.hpp>
#include <Euclid/BoundingVolume/OBB.h>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Math/Vector.h>
#include <Euclid/Render/RayTracer.h>

namespace Euclid
{

template<typename Mesh, typename T>
void proxy_view(const Mesh& mesh,
                const ViewSphere<Mesh>& view_sphere,
                std::vector<T>& view_scores,
                float weight)
{
    using Point_3 = typename boost::property_traits<
        typename boost::property_map<Mesh,
                                     boost::vertex_point_t>::type>::value_type;
    using Kernel = typename CGAL::Kernel_traits<Point_3>::Kernel;
    const int proxies = 6;
    const int width = 256;
    const int height = 256;
    const float least_visible =
        std::cos(boost::math::float_constants::pi / 3.0f);
    const float w1 = weight;    // weight for projected areas
    const float w2 = 1.0f - w1; // weight for visible ratios

    auto [vbeg, vend] = vertices(mesh);
    auto mesh_vpmap = get(boost::vertex_point, mesh);
    OBB<Kernel> obb;
    obb.build(vbeg, vend, mesh_vpmap);

    // Compute projected area
    std::vector<float> positions;
    std::vector<unsigned> indices;
    extract_mesh<3>(mesh, positions, indices);
    positions.push_back(0.0f); // Embree alignment
    RayTracer raytracer;
    raytracer.attach_geometry_buffers(positions, indices);

    std::vector<float> projected_areas(proxies);
    for (size_t i = 0; i < projected_areas.size(); ++i) {
        OrthoRayCamera cam;
        auto view_dir = view_sphere.radius * obb.axis(i % 3);
        if (i >= 3) {
            view_dir = -view_dir;
        }
        Eigen::Vector3f pos, focus, up;
        cgal_to_eigen(view_sphere.center + view_dir, pos);
        cgal_to_eigen(view_sphere.center, focus);
        cgal_to_eigen(obb.axis((i + 1) % 3), up);
        cam.lookat(pos, focus, up);
        cam.set_extent(view_sphere.radius * 2.0f, view_sphere.radius * 2.0f);

        std::vector<uint8_t> pixels(width * height, 0);
        raytracer.render_silhouette(pixels, cam, width, height);
        auto proj = 0;
        for (size_t j = 0; j < pixels.size(); ++j) {
            if (pixels[j] != 0) {
                ++proj;
            }
        }
        projected_areas[i] = static_cast<float>(proj) / pixels.size();
    }
    auto max_proj_area =
        *std::max_element(projected_areas.begin(), projected_areas.end());
    for (auto& proj_area : projected_areas) {
        proj_area /= max_proj_area;
    }

    // Compute visible ratio
    std::vector<float> visible_ratios(proxies);
    std::vector<int> n_visible_facets(proxies, 0);
    for (const auto& f : faces(mesh)) {
        auto normal = face_normal(f, mesh);
        if (normal * obb.axis(0) > least_visible) {
            ++n_visible_facets[0];
        }
        if (normal * obb.axis(1) > least_visible) {
            ++n_visible_facets[1];
        }
        if (normal * obb.axis(2) > least_visible) {
            ++n_visible_facets[2];
        }
        if (-normal * obb.axis(0) > least_visible) {
            ++n_visible_facets[3];
        }
        if (-normal * obb.axis(1) > least_visible) {
            ++n_visible_facets[4];
        }
        if (-normal * obb.axis(2) > least_visible) {
            ++n_visible_facets[5];
        }
    }
    float inv_nf = 1.0f / num_faces(mesh);
    std::transform(n_visible_facets.begin(),
                   n_visible_facets.end(),
                   visible_ratios.begin(),
                   [inv_nf](int nf) { return nf * inv_nf; });
    auto max_visible_ratio =
        *std::max_element(visible_ratios.begin(), visible_ratios.end());
    for (auto& visible_ratio : visible_ratios) {
        visible_ratio /= max_visible_ratio;
    }

    // Compute final score
    view_scores.clear();
    view_scores.resize(num_vertices(view_sphere.mesh), 0.0f);
    auto sphere_vpmap = get(boost::vertex_point, view_sphere.mesh);
    auto sphere_vimap = get(boost::vertex_index, view_sphere.mesh);
    for (const auto& v : vertices(view_sphere.mesh)) {
        auto view_dir = normalized(get(sphere_vpmap, v) - obb.center());
        auto i = get(sphere_vimap, v);
        view_scores[i] += (w1 * projected_areas[0] + w2 * visible_ratios[0]) *
                          std::max(view_dir * obb.axis(0), 0.0f);
        view_scores[i] += (w1 * projected_areas[1] + w2 * visible_ratios[1]) *
                          std::max(view_dir * obb.axis(1), 0.0f);
        view_scores[i] += (w1 * projected_areas[2] + w2 * visible_ratios[2]) *
                          std::max(view_dir * obb.axis(2), 0.0f);
        view_scores[i] += (w1 * projected_areas[3] + w2 * visible_ratios[3]) *
                          std::max(-view_dir * obb.axis(0), 0.0f);
        view_scores[i] += (w1 * projected_areas[4] + w2 * visible_ratios[4]) *
                          std::max(-view_dir * obb.axis(1), 0.0f);
        view_scores[i] += (w1 * projected_areas[5] + w2 * visible_ratios[5]) *
                          std::max(-view_dir * obb.axis(2), 0.0f);
    }
}

} // namespace Euclid


#include "scaled_variance.h"

#include "smash/../../scatteractionsfinder.cc"

#include <Eigen/Dense>
#include <fstream>
#include <gsl/gsl_sf_bessel.h>

/// Loads particle table from SMASH
bool load_particle_types() {
  std::cout << "Loading SMASH particle types and decay modes" << std::endl;
  std::string smash_dir(std::getenv("SMASH_DIR"));
  if (smash_dir == "") {
    std::cerr << "Failed to load SMASH particle types. SMASH_DIR is not set."
              << std::endl;
    return true;
  }
  std::ifstream particles_input_file(smash_dir + "/input/particles.txt");
  std::stringstream buffer;
  if (particles_input_file) {
    buffer << particles_input_file.rdbuf();
    smash::ParticleType::create_type_list(buffer.str());
  } else {
    std::cerr << "File with SMASH particle list not found." << std::endl;
    return true;
  }
  std::ifstream decaymodes_input_file(smash_dir + "/input/decaymodes.txt");
  if (decaymodes_input_file) {
    buffer.clear();
    buffer.str(std::string());
    buffer << decaymodes_input_file.rdbuf();
    smash::DecayModes::load_decaymodes(buffer.str());
    smash::ParticleType::check_consistency();
  } else {
    std::cerr << "File with SMASH decaymodes not found." << std::endl;
    return true;
  }
  return false;
}

int ScaledVarianceCalculator::set_eos_solver_equations(
                             const gsl_vector* x, void* params,
                             gsl_vector* f) {
  smash::ParticleTypePtrList* types =
      reinterpret_cast<struct solver_params *>(params)->types;
  double e = reinterpret_cast<struct solver_params *>(params)->e;
  double nb = reinterpret_cast<struct solver_params *>(params)->nb;
  double ns = reinterpret_cast<struct solver_params *>(params)->ns;
  double nq = reinterpret_cast<struct solver_params *>(params)->nq;

  const double T = gsl_vector_get(x, 0);
  const double mub = gsl_vector_get(x, 1);
  const double mus = gsl_vector_get(x, 2);
  const double muq = gsl_vector_get(x, 3);

  double e_tot = 0.0, nb_tot = 0.0, ns_tot = 0.0, nq_tot = 0.0;
  for (const smash::ParticleTypePtr t : (*types)) {
    const double b = t->baryon_number();
    const double s = t->strangeness();
    const double q = t->charge();
    const double z = t->mass() / T;
    const double mu = mub * b + mus * s + muq * q;
    const double mu_m_over_T = (mu - t->mass()) / T;
    const double factor = t->pdgcode().spin_degeneracy() * 4.0 * M_PI *
                          std::pow(T / (2.0 * M_PI * smash::hbarc), 3);
    const double k1 = gsl_sf_bessel_Kn_scaled(1, z);
    const double k2 = gsl_sf_bessel_Kn_scaled(2, z);
    const double ex = std::exp(mu_m_over_T);
    double rho = z * z *  k2 * ex * factor;
    double edens = z * z * (3 * k2 + z * k1) * ex * T * factor;
    e_tot += edens;
    nb_tot += rho * b;
    ns_tot += rho * s;
    nq_tot += rho * q;
  }
  gsl_vector_set(f, 0, e_tot - e);
  gsl_vector_set(f, 1, nb_tot - nb);
  gsl_vector_set(f, 2, ns_tot - ns);
  gsl_vector_set(f, 3, nq_tot - nq);
  return GSL_SUCCESS;
}

std::string ScaledVarianceCalculator::print_solver_state(size_t iter,
     gsl_multiroot_fsolver* solver) const {
  std::stringstream s;
  // clang-format off
  s << "iter = " << iter << ","
    << " x = " << gsl_vector_get(solver->x, 0) << " "
               << gsl_vector_get(solver->x, 1) << " "
               << gsl_vector_get(solver->x, 2) << " "
               << gsl_vector_get(solver->x, 3) << ", "
    << "f(x) = " << gsl_vector_get(solver->f, 0) << " "
                 << gsl_vector_get(solver->f, 1) << " "
                 << gsl_vector_get(solver->f, 2) << " "
                 << gsl_vector_get(solver->f, 3) << " "
                 << std::endl;
  // clang-format on
  return s.str();

}

void ScaledVarianceCalculator::setTmu_from_conserved(double Etot, double V,
                                                double B, double S, double Q) {
  if (quantum_statistics_) {
    std::cout << "WARNING: quantum statistics requested, but not implemented "
              << "for (E, B, S, Q) -> (T, muB, muS, muQ) solver." << std::endl;
    throw std::runtime_error("");
  }
  const gsl_multiroot_fsolver_type *solver_type = gsl_multiroot_fsolver_hybrid;
  gsl_multiroot_fsolver* solver = gsl_multiroot_fsolver_alloc(solver_type, 4);
  gsl_vector* x = gsl_vector_alloc(4);
  int residual_status = GSL_SUCCESS;
  size_t iter = 0;
  std::cout << print_solver_state(iter, solver);

  struct solver_params p = {&all_types_in_the_box_, Etot/V, B/V, S/V, Q/V};
  gsl_multiroot_function f = {&set_eos_solver_equations, 4, &p};

  gsl_vector_set(x, 0, 0.15);
  gsl_vector_set(x, 1, 0.01);
  gsl_vector_set(x, 2, 0.01);
  gsl_vector_set(x, 3, 0.01);

  gsl_multiroot_fsolver_set(solver, &f, x);
  do {
    iter++;
    const auto iterate_status = gsl_multiroot_fsolver_iterate(solver);
    // std::cout << print_solver_state(iter, solver);

    // Avoiding too low temperature
    if (gsl_vector_get(solver->x, 0) < 0.015) {
      T_ = 0.0;
      mub_ = 0.0;
      mus_ = 0.0;
      muq_ = 0.0;
      return;
    }

    // check if solver is stuck
    if (iterate_status) {
      break;
    }
    residual_status = gsl_multiroot_test_residual(solver->f, 1.e-9);
  } while (residual_status == GSL_CONTINUE && iter < 1000);

  if (residual_status != GSL_SUCCESS) {
    std::stringstream solver_parameters;
    solver_parameters << "Solver run with "
                      << "e = " << Etot/V << ", nb = " << B/V
                      << ", ns = " << S/V << ", nq = " << Q/V
                      << std::endl;
    throw std::runtime_error(gsl_strerror(residual_status) +
                             solver_parameters.str() +
                             print_solver_state(iter, solver));
  }
  T_ = gsl_vector_get(solver->x, 0);
  mub_ = gsl_vector_get(solver->x, 1);
  mus_ = gsl_vector_get(solver->x, 2);
  muq_ = gsl_vector_get(solver->x, 3);
  gsl_multiroot_fsolver_free(solver);
  gsl_vector_free(x);
}

std::pair<double, double> ScaledVarianceCalculator::scaled_variance(
    std::function<bool(const smash::ParticleTypePtr)> type_of_interest) {

  constexpr unsigned int m = 5;
  Eigen::MatrixXd k2_tilde = Eigen::MatrixXd::Zero(m, m);
  double kappa1 = 0.0;

  for (const smash::ParticleTypePtr ptype : all_types_in_the_box_) {
    const double b = B_conservation_ ? ptype->baryon_number() : 0;
    const double s = S_conservation_ ? ptype->strangeness() : 0;
    const double q = Q_conservation_ ? ptype->charge() : 0;
    const double z = ptype->mass() / T_;
    const double mu = mub_ * b + mus_ * s + muq_ * q;
    const double mu_m_over_T = (mu - ptype->mass()) / T_;
    if (mu_m_over_T > 0 and quantum_statistics_) {
      std::cout << "Warning: quantum expressions for " << ptype->name() <<
                   " do not converge, m < chemical potential." << std::endl;
    }
    const double factor = ptype->pdgcode().spin_degeneracy() * 4.0 * M_PI *
                          std::pow(T_ / (2.0 * M_PI * smash::hbarc), 3);
    double EE = 0.0, EN = 0.0, NN = 0.0, N1 = 0.0;
    // std::cout << "Computing matrix for " << ptype.name() << std::endl;
    for (unsigned int k = 1; k < quantum_series_max_terms_; k++) {
      if (k > 1 and !quantum_statistics_) {
        break;
      }
      const double k1 = gsl_sf_bessel_Kn_scaled(1, z * k);
      const double k2 = gsl_sf_bessel_Kn_scaled(2, z * k);
      const double x = std::exp(mu_m_over_T * k);
      double N1_summand = z * z / k * k2 * x;
      double NN_summand = z * z * k2 * x;
      double EN_summand = z * z / k * (3 * k2 + k * z * k1) * x;
      double EE_summand = z * z / (k * k) *
                          ((z * z * k * k + 12.0) * k2 + 3.0 * z * k * k1) * x;
      // std::cout << "k = " << k
      //          << ", N1_summand*factor*1000 = " << N1_summand*factor*1000
      //          << ", NN_summand*factor*1000 = " << NN_summand*factor*1000
      //          << ", EN_summand = " << EN_summand
      //          << ", EE_summand = " << EE_summand << std::endl;
      if (k > 1 and
          EE_summand < EE * quantum_series_rel_precision_ and
          EN_summand < EN * quantum_series_rel_precision_ and
          NN_summand < NN * quantum_series_rel_precision_ and
          N1_summand < N1 * quantum_series_rel_precision_) {
        break;
      }
      if (k % 2 == 0 and ptype->pdgcode().is_baryon()) {
        NN_summand = -NN_summand;
        EN_summand = -EN_summand;
        EE_summand = -EE_summand;
        N1_summand = -N1_summand;
      }
      NN += NN_summand;
      EN += EN_summand;
      EE += EE_summand;
      N1 += N1_summand;
    }
    EE *= factor;
    EN *= factor;
    NN *= factor;
    N1 *= factor;

    Eigen::MatrixXd k2_tilde_ptype(m, m);
    // clang-format off
    k2_tilde_ptype << NN,     EN,    q*NN,    b*NN,    s*NN,
                      EN,     EE,    q*EN,    b*EN,    s*EN,
                      q*NN, q*EN,  q*q*NN,  q*b*NN,  q*s*NN,
                      b*NN, b*EN,  b*q*NN,  b*b*NN,  b*s*NN,
                      s*NN, s*EN,  s*q*NN,  s*b*NN,  s*s*NN;
    // clang-format on
    if (!type_of_interest(ptype)) {
      k2_tilde_ptype.row(0).setZero();
      k2_tilde_ptype.col(0).setZero();
      N1 = 0.0;
    }
    kappa1 += N1;
    k2_tilde += k2_tilde_ptype;
  }

  if (!energy_conservation_) {
    k2_tilde.row(1).setZero();
    k2_tilde.col(1).setZero();
  }
  // Remove zero rows and columns before asking for determinant
  // This uses the property of k2_tilde matrix being symmetric
  Eigen::Matrix<bool, 1, Eigen::Dynamic> non_zeros =
      k2_tilde.cast<bool>().colwise().any();
  const unsigned int n = non_zeros.count();
  Eigen::MatrixXd k2_tilde_nz(n, n);
  int i = 0, j;
  for (unsigned int i0 = 0; i0 < m; ++i0) {
    if (!non_zeros(i0)) {
      continue;
    }
    j = 0;
    for (unsigned int j0 = 0; j0 < m; ++j0) {
      if (non_zeros(j0)) {
        k2_tilde_nz(i, j) = k2_tilde(i0, j0);
        j++;
      }
    }
    i++;
  }

  const double rho_type_interest = kappa1;
  // No conservation laws: grand-canonical case
  if (n < 2) {
    return std::make_pair(rho_type_interest,
                          k2_tilde_nz(0,0) / kappa1);
  }

  const double det_k2_tilde = k2_tilde_nz.determinant();
  const double det_k2 = k2_tilde_nz.block(1, 1, n - 1, n - 1).determinant();
  const double expected_scaled_variance = det_k2_tilde / det_k2 /
                                          rho_type_interest;
  return std::make_pair(rho_type_interest, expected_scaled_variance);
}

void ScaledVarianceCalculator::prepare_decays() {
  std::vector<smash::ParticleTypePtr> all_stable_hadrons;
  for (const smash::ParticleType &ptype : smash::ParticleType::list_all()) {
    if (ptype.is_stable()) {
      all_stable_hadrons.push_back(&ptype);
    }
  }

  for (const smash::ParticleTypePtr res : all_types_in_the_box_) {
    smash::decaytree::Node tree(res->name(), 1.0, {res}, {res}, {res}, {});
    constexpr double sqrts = 5.0;  // This is sufficient to add all possible decays
    smash::decaytree::add_decays(tree, sqrts);
    std::vector<smash::FinalStateCrossSection> fs = tree.final_state_cross_sections();
    smash::deduplicate(fs);
    double wsum = 0.0;
    for (const smash::FinalStateCrossSection &xs : fs) {
      wsum += xs.cross_section_;
    }
    assert(std::abs(wsum - 1.0) < 1.e-9);
    std::cout << res->name() << std::endl;
    for (const smash::FinalStateCrossSection &xs : fs) {
      std::map<smash::ParticleTypePtr, int> decay_final_states;
      // For each stable particle count how many of it one finds in the final state
      for (const smash::ParticleTypePtr ptype : all_stable_hadrons) {
        std::string little_str = ptype->name(),
                    big_str = xs.name_;
        // How many times little_str occurs in big_str?
        int nPos = big_str.find(little_str, 0);
        int count = 0;
        while (nPos != std::string::npos) {
          count++;
          nPos = big_str.find(little_str, nPos + little_str.size());
        }
        if (count > 0) {
          decay_final_states[ptype] = count;
        }
      }
      all_decay_final_states_[res].emplace_back(
          std::make_pair(xs.cross_section_, decay_final_states));
      std::cout << "    " << xs.name_ << " " << xs.cross_section_ << ";   ";
      for (const auto &hadron_count : decay_final_states) {
        std::cout << hadron_count.second << hadron_count.first->name() << " ";
      }
      std::cout << std::endl;
    }
  }
}

std::ostream& operator<< (std::ostream& out,
                          const ScaledVarianceCalculator &svc) {
  out << "Scaled variance calculator" << std::endl;
  out << "T [GeV] = " << svc.T_
      << ", muB [GeV] = " << svc.mub_
      << ", muS [GeV] = " << svc.mus_
      << ", muQ [GeV] = " << svc.muq_ << std::endl;
  out << "Quantum statistics = " << svc.quantum_statistics_ << std::endl;
  out << "Included conservation laws: ";
  if (svc.energy_conservation_) {
    out << " energy;";
  }
  if (svc.B_conservation_) {
    out << "  baryon number;";
  }
  if (svc.S_conservation_) {
    out << " strangeness;";
  }
  if (svc.Q_conservation_) {
    out << " electric charge;";
  }
  out << std::endl;
  out << "Species in the box:" << std::endl;
  for (const smash::ParticleTypePtr t : svc.all_types_in_the_box_) {
    out << t->name() << " ";
  }
  out << std::endl;
  return out;
}

int main() {
  load_particle_types();

  // Prepare the set of particle species in the box
  std::vector<smash::ParticleTypePtr> hadrons_in_the_box;
  for (const smash::ParticleType &t : smash::ParticleType::list_all()) {
    if (t.is_hadron() && t.mass() < 2.0) {
      hadrons_in_the_box.push_back(&t);
    }
  }
  std::sort(hadrons_in_the_box.begin(), hadrons_in_the_box.end(),
            [&](smash::ParticleTypePtr ta, smash::ParticleTypePtr tb) {
              return ta->mass() < tb->mass();
            });

  const double Temperature = 0.2;  // [GeV]
  const double muB = 0.0;  // [GeV]
  const double muS = 0.0;  // [GeV]
  const double muQ = 0.0;  // [GeV]
  const bool E_conservation = true;
  const bool B_conservation = true;
  const bool S_conservation = true;
  const bool Q_conservation = true;
  const bool quantum_statistics = false;
  ScaledVarianceCalculator svc(hadrons_in_the_box,
                               Temperature, muB, muS, muQ,
                               E_conservation, B_conservation,
                               S_conservation, Q_conservation,
                               quantum_statistics);
  svc.prepare_decays();
/*
  const double V = 1762.1897;  // [fm^3]
  const double E_tot = 972.4227;  // [GeV]
  const double B_tot = 0.0;
  const double S_tot = 0.0;
  const double Q_tot = 0.0;
  svc.setTmu_from_conserved(E_tot, V, B_tot, S_tot, Q_tot);
  std::cout << svc;

  // Variance of each specie
  for (const smash::ParticleTypePtr t : hadrons_in_the_box) {
    const auto density_and_variance = svc.scaled_variance(
          [&](const smash::ParticleTypePtr t0) { return t0 == t; });
    std::cout << t->name() << " " << density_and_variance.first * V << " "
              << density_and_variance.second << std::endl;
  }

  // Variance of total number
  const auto density_and_variance = svc.scaled_variance(
      [&](const smash::ParticleTypePtr) { return true; });
  std::cout << "Ntot " << density_and_variance.first * V << " "
            << density_and_variance.second << std::endl;
*/
}


#include <chrono>
#include <iostream>

#include <Eigen/Core>

#include "ldaplusplus/e_step_utils.hpp"

using namespace Eigen;
using namespace ldaplusplus;

int main(int argc, char **argv) {
    VectorXd X = VectorXd::Random(1000);
    X.array() -= X.minCoeff();
    X.array() /= X.sum();
    VectorXi X_counts = (X*1000).cast<int>();

    MatrixXd eta = MatrixXd::Random(600, 10);

    MatrixXd phi = MatrixXd::Random(600, 1000);
    phi.array() -= phi.minCoeff();
    phi.array().rowwise() /= phi.array().colwise().sum();

    VectorXd h(phi.rows());

    // Warm up the cache
    for (int i=0; i<10; i++) {
        e_step_utils::compute_h<double>(X_counts, X, eta, phi, h);
    }

    std::chrono::high_resolution_clock clock;
    std::chrono::high_resolution_clock::duration s(0);
    for (int i=0; i<100; i++) {
        auto start = clock.now();
        e_step_utils::compute_h<double>(X_counts, X, eta, phi, h);
        s += clock.now() - start;
    }

    std::cout << std::chrono::duration_cast<std::chrono::duration<double, std::ratio<1> > >(s).count() << "s" << std::endl;

    return 0;
}


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostBrandesCentrality.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
#include "vtkBoostBrandesCentrality.h"

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkMath.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkFloatArray.h"
#include "vtkDataArray.h"
#include "vtkSmartPointer.h"
#include "vtkStringArray.h"

#include "vtkBoostGraphAdapter.h"
#include "vtkDirectedGraph.h"
#include "vtkUndirectedGraph.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/properties.hpp>

using namespace boost;

vtkStandardNewMacro(vtkBoostBrandesCentrality);

//-----------------------------------------------------------------------------
vtkBoostBrandesCentrality::vtkBoostBrandesCentrality() :
  UseEdgeWeightArray    (false),
  InvertEdgeWeightArray (false),
  EdgeWeightArrayName   (NULL)
{
}

//-----------------------------------------------------------------------------
vtkBoostBrandesCentrality::~vtkBoostBrandesCentrality()
{
  this->SetEdgeWeightArrayName(0);
}

//-----------------------------------------------------------------------------
void vtkBoostBrandesCentrality::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "UseEdgeWeightArray: " << this->UseEdgeWeightArray << endl;

  os << indent << "InvertEdgeWeightArray: " << this->InvertEdgeWeightArray
    << endl;

  os << indent << "this->EdgeWeightArrayName: " <<
    (this->EdgeWeightArrayName ?  this->EdgeWeightArrayName : "NULL") << endl;
}

//-----------------------------------------------------------------------------
int vtkBoostBrandesCentrality::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkGraph *input = vtkGraph::SafeDownCast(
    inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkGraph *output = vtkGraph::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));

  // Send the data to output.
  output->ShallowCopy(input);

  // Compute betweenness centrality

  // Property map for vertices
  vtkFloatArray* vertexCMap = vtkFloatArray::New();
  vertexCMap->SetName("centrality");
  identity_property_map imap;

  // Property map for edges
  vtkFloatArray* edgeCMap = vtkFloatArray::New();
  edgeCMap->SetName("centrality");
  vtkGraphEdgePropertyMapHelper<vtkFloatArray*> helper(edgeCMap);

  vtkSmartPointer<vtkDataArray> edgeWeight (0);
  if(this->UseEdgeWeightArray && this->EdgeWeightArrayName)
  {
    if(!this->InvertEdgeWeightArray)
    {
      edgeWeight = input->GetEdgeData()->GetArray(this->EdgeWeightArrayName);
    }
    else
    {
      vtkDataArray* weights =
          input->GetEdgeData()->GetArray(this->EdgeWeightArrayName);

      if(!weights)
      {
        vtkErrorMacro(<<"Error: Edge weight array " << this->EdgeWeightArrayName
                      << " is set but not found or not a data array.\n");
        return 0;
      }

      edgeWeight.TakeReference(
        vtkDataArray::CreateDataArray(weights->GetDataType()));

      double range[2];
      weights->GetRange(range);

      if(weights->GetNumberOfComponents() > 1)
      {
        return 0;
      }

      for(int i=0; i < weights->GetDataSize(); ++i)
      {
        edgeWeight->InsertNextTuple1(range[1] - weights->GetTuple1(i));
      }
    }

    if(!edgeWeight)
    {
      vtkErrorMacro(<<"Error: Edge weight array " << this->EdgeWeightArrayName
                    << " is set but not found or not a data array.\n");
      return 0;
    }
  }

  // Is the graph directed or undirected
  if (vtkDirectedGraph::SafeDownCast(output))
  {
    vtkDirectedGraph *g = vtkDirectedGraph::SafeDownCast(output);
    if(edgeWeight)
    {
      vtkGraphEdgePropertyMapHelper<vtkDataArray*> helper2(edgeWeight);
      brandes_betweenness_centrality(g,
        centrality_map(vertexCMap).edge_centrality_map(
          helper).vertex_index_map(imap).weight_map(helper2));
    }
    else
    {
      brandes_betweenness_centrality(g,
        centrality_map(vertexCMap).edge_centrality_map(
          helper).vertex_index_map(imap));
    }
  }
  else
  {
    vtkUndirectedGraph *g = vtkUndirectedGraph::SafeDownCast(output);
    if(edgeWeight)
    {
      vtkGraphEdgePropertyMapHelper<vtkDataArray*> helper2(edgeWeight);
      brandes_betweenness_centrality(g,
             centrality_map(vertexCMap).edge_centrality_map(
               helper).vertex_index_map(imap).weight_map(helper2));
    }
    else
    {
      brandes_betweenness_centrality(g,
             centrality_map(vertexCMap).edge_centrality_map(
               helper).vertex_index_map(imap));
    }
  }

  // Add the arrays to the output and dereference
  output->GetVertexData()->AddArray(vertexCMap);
  vertexCMap->Delete();
  output->GetEdgeData()->AddArray(edgeCMap);
  edgeCMap->Delete();

  return 1;
}


#ifndef LEPH_HUMANOIDMODEL_HPP
#define LEPH_HUMANOIDMODEL_HPP

#include <Eigen/Dense>
#include "LegIK.hpp"

namespace Rhoban {

/**
 * Leg motor result positions
 */
struct IKWalkOutputs {
    double left_hip_yaw;
    double left_hip_roll;
    double left_hip_pitch;
    double left_knee;
    double left_ankle_pitch;
    double left_ankle_roll;
    double right_hip_yaw;
    double right_hip_roll;
    double right_hip_pitch;
    double right_knee;
    double right_ankle_pitch;
    double right_ankle_roll;
};

}

namespace Leph {

/**
 * All combinations
 * of Euler angles types
 * in same order as rotation application
 *
 * EulerYawPitchRoll is built as
 * Roll * Pitch * Yaw.
 */
enum EulerType {
    EulerYawPitchRoll,
    EulerYawRollPitch,
    EulerRollPitchYaw,
    EulerRollYawPitch,
    EulerPitchRollYaw,
    EulerPitchYawRoll,
};

/**
 * HumanoidModel
 */
class HumanoidModel
{
    public:

        /**
         * Initialize the model with given
         * typical leg length
         */
        HumanoidModel(
            double distHipToKnee,
            double distKneeToAnkle,
            double distAnkleToGround,
            double distFeetLateral);

        /**
         * Run analytical inverse kinematics LegIK and
         * assign outputs motor positions.
         * Target footPos and angles are given in
         * special frame when all leg motors are in zero position.
         * Frame center is located on left or right foot tip.
         * True is returned if angles are updated and inverse
         * kinematics is sucessful, else false is returned.
         */
        bool legIkLeft(
            const Eigen::Vector3d& footPos,
            const Eigen::Vector3d& angles,
            EulerType eulerType,
            Rhoban::IKWalkOutputs& outputs);
        bool legIkRight(
            const Eigen::Vector3d& footPos,
            const Eigen::Vector3d& angles,
            EulerType eulerType,
            Rhoban::IKWalkOutputs& outputs);

        /**
         * Return the initial vertical distance
         * from trunk frame to foot tip frame (Z)
         */
        double legsLength() const;

        /**
         * Return the initial lateral distance
         * between each feet
         */
        double feetDistance() const;

    private:

        /**
         * Leg segments lengths used by
         * inverse kinematics
         */
        double _legHipToKnee;
        double _legKneeToAnkle;
        double _legAnkleToGround;
        double _legLateral;

        /**
         * Convert given euler angle of given
         * convention type to rotation matrix
         */
        Eigen::Matrix3d eulersToMatrix(
            const Eigen::Vector3d angles, EulerType eulerType) const;

        /**
         * Compute and return the IK position reference
         * vector and orientation reference matrix
         * in LegIK specifics structures
         */
        LegIK::Vector3D buildTargetPos(
            const Eigen::Vector3d& footPos);
        LegIK::Frame3D buildTargetOrientation(
            const Eigen::Vector3d& angles,
            EulerType eulerType);

        /**
         * Assign model leg DOF to given IK results
         */
        void setIKResult(
            const LegIK::Position& result, bool isLeftLeg,
            Rhoban::IKWalkOutputs& outputs);

        /**
         * Check inverse kinematics computed value
         * and throw an error in case of NaN
         */
        void checkNaN(
            const LegIK::Position& result,
            const LegIK::Vector3D& pos,
            const LegIK::Frame3D& orientation) const;
};

}

#endif


#include <Eigen/Core>
#include <iterator>
#include <set>
#include <vector>

#include "test/catch.hpp"
#include "rose499/world.hpp"

SCENARIO( "a world with boxes", "[world]" )
{
    GIVEN( "a world with a single box" )
    {
        World world;
        world.addBoxObstacle(0, 1, 1, 10, 10);

        WHEN( "observing the world while facing the box" )
        {
            World::Polygon viewFrustum;
            geom::append(viewFrustum.outer(), World::Point(0.0, 0.0));
            geom::append(viewFrustum.outer(), World::Point(2.0, 0.0));
            geom::append(viewFrustum.outer(), World::Point(0.0, 2.0));
            geom::append(viewFrustum.outer(), World::Point(0.0, 0.0));
            auto obstacles = world.observeWorld(viewFrustum);

            THEN( "the box is observed" )
            {
                REQUIRE( !obstacles.empty() );
                REQUIRE( obstacles.front().second == 0 );
            }
        }

        WHEN( "observing the world while not facing the box" )
        {
            World::Polygon viewFrustum;
            geom::append(viewFrustum.outer(), World::Point(12.0, 12.0));
            geom::append(viewFrustum.outer(), World::Point(12 + 2.0, 12.0));
            geom::append(viewFrustum.outer(), World::Point(12.0, 12 + 2.0));
            geom::append(viewFrustum.outer(), World::Point(12.0, 12.0));
            auto obstacles = world.observeWorld(viewFrustum);

            THEN( "the box is not observed" )
            {
                REQUIRE( obstacles.empty() );
            }
        }
    }

    GIVEN( "a world with many boxes" )
    {
        World world;
        int i = 0;
        for(i = 0; i < 10; i++)
            world.addBoxObstacle(i, 1 + 12 * i, 5, 5, 5);
        world.addBoxObstacle(i, 0, 20, 95, 10);

        WHEN( "planning a path from [0,0], to [95,95]" )
        {
            auto path = planRRT( world.sceneTree(),     // Scene Tree (R-Tree)
                                 100, 100,              // Bounds of the Scene
                                 World::Point(0, 0),    // Initial condition
                                 0,                     // UNUSED: Direction
                                 World::Point(95, 95),  // Goal Point
                                 5                      // Radius tolerance for achieving goal.
                               );

            THEN( "there exists a non-empty path to the goal" )
            {
                REQUIRE( path.size() > 1 );

                THEN( "the path starts at the initial point and ends in the goal region" )
                {
                    auto start = path.front();
                    auto end = path.back();
                    REQUIRE( Eigen::Vector2d( start.get<0>(), start.get<1>() ).norm() < 1e-6 );
                    REQUIRE( Eigen::Vector2d( end.get<0>() - 95, end.get<1>() - 95 ).norm() <= 5 );
                }

                THEN( "the (segmented) path doesn't intersect any of the boxes in our world" )
                {
                    std::vector< std::pair<World::Box, int> > setCollisions;
                    auto start = std::begin(path);
                    auto end = std::next(start);
                    do {
                        geom::model::segment< World::Point > segment(*start, *end);
                        world.sceneTree().query(
                            geom::index::intersects(segment),
                            std::back_inserter(setCollisions)
                        );
                        REQUIRE(setCollisions.empty());

                        start = end;
                        end = std::next(end);
                    } while( end != std::end(path) );
                }
            }
        }
    }
}


/*
 * LYPokerAlgorithmDelegate.cpp
 *
 *  Created on: 2013-7-31
 *      Author: caiqingfeng
 */

#include <boost/random.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/regex.hpp>

#include "LYPokerAlgorithmDelegate.h"

typedef boost::mt19937 LYRng; //random number generator

LYPokerAlgorithmDelegate::LYPokerAlgorithmDelegate() {
	// TODO Auto-generated constructor stub

}

LYPokerAlgorithmDelegate::~LYPokerAlgorithmDelegate() {
	// TODO Auto-generated destructor stub
}

void LYPokerAlgorithmDelegate::doShuffle(std::vector<LYCard> &deck)
{
	LYRng rng(std::time(0)); //must set a seed, otherwise always get the same result
	boost::uniform_int<> deckRange(1, deck.size());
	boost::variate_generator<LYRng, boost::uniform_int<> > oneDeck(rng, deckRange);
	std::random_shuffle(deck.begin(), deck.end(), oneDeck);
}