/*
 * Copyright (C) 2017 roman <roman.podolski@tum.de>
 *
 * Distributed under terms of the MIT license.
 */

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <boost/geometry.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/random/normal_distribution.hpp>
#include <cmath>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <iostream>
#include <random>
#include <vector>
#include <chrono>
#include "planner/car.h"
#include "planner/io/beamer.h"
#include "planner/io/tex.h"
#include "planner/planner.h"

DEFINE_string(filename, "simulation.tex", "name of file");
DEFINE_double(road_width, 3.0, "Road with");
DEFINE_double(cone_dist, 5.0, "Cone dist");
DEFINE_double(cone_radius, 0.15, "Cone dist");
DEFINE_double(cone_sigma, 0.5, "cone placement standart deviation");
DEFINE_double(velocity, 5, "velocity of car");
DEFINE_double(frequency, 5, "update frequency");
DEFINE_double(position_sigma, 0.01, "standart deviation of the position");
DEFINE_double(heading_sigma, 0.01, "standart deviation of the heading");

using std::milli;
using boost::math::constants::pi;
using boost::math::constants::half_pi;
using boost::math::constants::third_pi;
using boost::math::constants::sixth_pi;
using boost::math::constants::two_pi;
using boost::random::normal_distribution;

int main(int argc, char *argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);

  FLAGS_number_maneuvers = 20;      //30
  FLAGS_max_maneuver_offset = 4;    //4
  FLAGS_granularity = 2;            //1
  FLAGS_min_maneuver_length = 15;   //20
  FLAGS_maneuver_speed_gain = 1;    //1
  FLAGS_max_curvature = 0.5;        //0.5

  std::vector<std::pair<double, double>> waypoints{
      {0, 0},    {10, 0},   {20, 0},   {30, 10},  {40, 20},  {40, 30},
      {40, 40},  {40, 50},  {40, 60},  {20, 70},  {10, 80},  {0, 80},
      {-10, 70}, {-20, 70}, {-30, 75}, {-40, 80}, {-50, 80}, {-60, 70},
      {-60, 60}, {-60, 50}, {-50, 40}, {-45, 30}, {-40, 20}, {-30, 10},
      {-20, 0},  {-10, 0},  {0, 0}};
  double road_width = FLAGS_road_width;
  double cone_radius = FLAGS_cone_radius;
  double cone_dist = FLAGS_cone_dist;
  double cone_sigma = FLAGS_cone_sigma;

/*  double L = 316;
  std::vector<std::pair<double, double>> waypoints = {};
  for (double i = 0; i < L; i++) {
    waypoints.push_back(std::make_pair(i, 0));
  }
*/
  std::ofstream tex(FLAGS_filename);
  planner::io::beamer_mapper mapper(tex, "xmin=-70,xmax=50,ymin=-10,ymax=90");

  planner::Planner jack(waypoints);

  std::vector<planner::polygon> obstacles;
  std::mt19937 rnd;

  int cone_count = 0;
  double x1, x2, y1, y2;
  for (int j = 1; j < jack.baseframe().length() - 1; j += cone_dist) {
    auto point_1 = jack.baseframe().P(j - 0.1);
    auto point_2 = jack.baseframe().P(j + 0.1);

    Eigen::Vector2d v_1(point_1.x(), point_1.y());
    Eigen::Vector2d v_2(point_2.x(), point_2.y());

    Eigen::Vector2d n = v_2 - v_1;
    n.normalize();

    double x_left = jack.baseframe().P(j).x() + road_width * n[1];
    double y_left = jack.baseframe().P(j).y() - road_width * n[0];
    double x_right = jack.baseframe().P(j).x() - road_width * n[1];
    double y_right = jack.baseframe().P(j).y() + road_width * n[0];

    normal_distribution<> dist_x_left(x_left, cone_sigma);
    normal_distribution<> dist_y_left(y_left, cone_sigma);
    normal_distribution<> dist_x_right(x_right, cone_sigma);
    normal_distribution<> dist_y_right(y_right, cone_sigma);

    x_left = dist_x_left(rnd);
    y_left = dist_y_left(rnd);
    x_right = dist_x_right(rnd);
    y_right = dist_y_right(rnd);

    planner::polygon p1, p2;
    for (double i = 0; i < two_pi<double>(); i += sixth_pi<double>()) {
      x1 = x_left + cone_radius * cos(i);
      y1 = y_left + cone_radius * sin(i);
      x2 = x_right + cone_radius * cos(i);
      y2 = y_right + cone_radius * sin(i);
      p1.outer().push_back(planner::point(x1, y1));
      p2.outer().push_back(planner::point(x2, y2));
    }
    obstacles.push_back(p1);
    obstacles.push_back(p2);
    cone_count += 2;
  }

  std::cout << "Placed " << cone_count << " cones." << '\n';
  std::cout << "Baseframe length: " << jack.baseframe().length() << '\n';

  planner::Car car;

  double x = waypoints[0].first, y = waypoints[0].second,
         heading = sixth_pi<double>();

  double s = 0.0;

  std::vector<planner::pose> optimal_path;
  int i = 0;
  while (s <= jack.baseframe().length() - 1) {
    std::cout << s << '\n';
    i++;
    for (auto o : obstacles) {
      mapper.map(o);
    }

    mapper.map(jack.baseframe(), "color=blue, dotted");

    planner::point position(x, y);
    planner::pose pos{position, heading, 0, 0};

    auto m_opt = jack.update(pos.position, pos.theta, FLAGS_velocity,
                             1.0 / FLAGS_frequency, obstacles);
    for (auto m : jack.maneuvers()) {
      if (m.collision()) {
        mapper.map(m, "color=red");
      } else {
        mapper.map(m, "color=black");
      }
    }
    mapper.map(m_opt, "color=green");
    mapper.map(optimal_path, "color=green");

    mapper.map(car.obb(pos));
    normal_distribution<> n_position_x(m_opt.path()[1].x(),
                                      FLAGS_position_sigma);
    normal_distribution<> n_position_y(m_opt.path()[1].y(),
                                      FLAGS_position_sigma);
    normal_distribution<> n_heading(m_opt.path()[1].theta, FLAGS_heading_sigma);
    optimal_path.push_back(m_opt.path()[1]);
    x = n_position_x(rnd);
    y = n_position_y(rnd);
    heading = n_heading(rnd);
//    x = m_opt.path()[1].position.x();
//    y = m_opt.path()[1].position.y();
//    heading = m_opt.path()[1].theta;
    s = m_opt.path()[1].s;
    mapper.next_frame();
  }

  for (auto o : obstacles) {
    // tex_m.map(o);
    mapper.map(o);
  }

  mapper.map(jack.baseframe(), "color=blue, dotted");
  mapper.map(optimal_path, "color=green");
}


/**
 * @file
 * @brief Tests for the clamp function
 * @copyright Copyright (c) 2020, New York University & Max Planck Gesellschaft.
 */
#include <gtest/gtest.h>
#include <Eigen/Eigen>

#include <robot_fingers/clamp.hpp>

TEST(TestClamp, clamp_within)
{
    Eigen::Vector3d vec(1, 2, 3);
    Eigen::Vector3d clamped = robot_fingers::clamp(vec, 0, 4);
    Eigen::Vector3d expected(1, 2, 3);
    ASSERT_EQ(expected, clamped);
}

TEST(TestClamp, clamp_below)
{
    Eigen::Vector3d vec(1, 2, 3);
    Eigen::Vector3d clamped = robot_fingers::clamp(vec, 2, 4);
    Eigen::Vector3d expected(2, 2, 3);
    ASSERT_EQ(expected, clamped);
}

TEST(TestClamp, clamp_above)
{
    Eigen::Vector4d vec(1, 2, 3, 4);
    Eigen::Vector4d clamped = robot_fingers::clamp(vec, 1, 3);
    Eigen::Vector4d expected(1, 2, 3, 3);
    ASSERT_EQ(expected, clamped);
}

TEST(TestClamp, clamp_below_and_above)
{
    Eigen::Vector4d vec(1, 2, 3, 4);
    Eigen::Vector4d clamped = robot_fingers::clamp(vec, 2, 3);
    Eigen::Vector4d expected(2, 2, 3, 3);
    ASSERT_EQ(expected, clamped);
}

int main(int argc, char **argv)
{
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012        MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <nt2/table.hpp>
#include <boost/fusion/include/at.hpp>
#include <nt2/include/functions/lu.hpp>
#include <nt2/include/functions/rand.hpp>
#include <nt2/include/functions/zeros.hpp>
#include "../../flops/lu.hpp"

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/gflops.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct lu1_nt2
{
  lu1_nt2(std::size_t n)
              :  size_(n)
  {
    result = nt2::zeros(n,n, nt2::meta::as_<T>());
    input  = nt2::rand(n,n, nt2::meta::as_<T>());
  }

  void operator()()
  {
    result = nt2::lu(input);
  }

  friend std::ostream& operator<<(std::ostream& os, lu1_nt2<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }
  std::size_t flops() const { return FLOPS_GETRF(size_,size_); }

private:
  std::size_t size_;
  nt2::table<T> input, result;
};

NT2_REGISTER_BENCHMARK_TPL( lu1_nt2, NT2_SIMD_REAL_TYPES )
{
  std::size_t size_min = args("size_min", 16);
  std::size_t size_max = args("size_max", 4096);
  std::size_t size_step = args("size_step", 10);

  run_during_with< lu1_nt2<T> > ( 1.
                                ,  geometric(size_min,size_max,size_step)
                                , gflops<stats::median_>()
                                );
}


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.

#ifndef poplibs_support_GraphUtil_hpp
#define poplibs_support_GraphUtil_hpp

#include <boost/graph/adjacency_matrix.hpp>
#include <vector>

// Returns an adjacency matrix representing pair-wise reachability of the input
// graph.
//
// Complexity is O(v^3) where v is the number of vertices in the given graph.
template <typename Graph>
static inline boost::adjacency_matrix<> pairwise_reachability(const Graph &g) {

  // Use a variant of the Floyd-Warshall algorithm to compute pairwise
  // reachability of the vertices in the graph.
  const auto numV = boost::num_vertices(g);
  std::vector<bool> r(numV * numV);

  // NOTE: Self-reachability is irrelevant to the algorithm.
  // The resulting graph defining reachability has a vertex v
  // adjacent to itself, i.e. reachable from itself.
  const auto &edges = boost::edges(g);
  for (auto it = edges.first; it != edges.second; ++it) {
    const auto u = boost::source(*it, g);
    const auto v = boost::target(*it, g);
    r[u * numV + v] = true;
  }

  // O(v^3) complexity comes from here.
  for (std::size_t k = 0; k < numV; ++k) {
    for (std::size_t u = 0; u < numV; ++u) {
      for (std::size_t v = 0; v < numV; ++v) {
        r[u * numV + v] =
            r[u * numV + v] || (r[u * numV + k] && r[k * numV + v]);
      }
    }
  }

  boost::adjacency_matrix<> result(numV);
  for (std::size_t u = 0; u < numV; ++u) {
    for (std::size_t v = 0; v < numV; ++v) {
      if (r[u * numV + v]) {
        boost::add_edge(u, v, result);
      }
    }
  }
  return result;
}

#endif // poplibs_support_GraphUtil_hpp


#include <big_types.h>
#include <big_project.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(big_project);

BOOST_AUTO_TEST_CASE(test_case)
{
  ublas::vector<double> x;

  x.resize(10,false);

  x(0) =  2.0;
  x(1) = -3.0;
  x(2) =  4.0;
  x(3) = -5.0;
  x(4) =  6.0;
  x(5) = -7.0;
  x(6) =  8.0;
  x(7) = -9.0;
  x(8) =  10.0;
  x(9) = -11.0;

  double const tol =0.01;
  ublas::vector<double>  y = x;

  big::project(x,y);

  BOOST_CHECK_CLOSE( y(0) , 2.0 , tol );
  BOOST_CHECK_CLOSE( y(1) , 0.0 , tol );
  BOOST_CHECK_CLOSE( y(2) , 4.0 , tol );
  BOOST_CHECK_CLOSE( y(3) , 0.0 , tol );
  BOOST_CHECK_CLOSE( y(4) , 6.0 , tol );
  BOOST_CHECK_CLOSE( y(5) , 0.0 , tol );
  BOOST_CHECK_CLOSE( y(6) , 8.0 , tol );
  BOOST_CHECK_CLOSE( y(7) , 0.0 , tol );
  BOOST_CHECK_CLOSE( y(8) , 10.0 , tol );
  BOOST_CHECK_CLOSE( y(9) , 0.0 , tol );

}

BOOST_AUTO_TEST_SUITE_END();


#include <stan/math/prim.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/VectorIntRNGTestRig.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

class BinomialTestRig : public VectorIntRNGTestRig {
 public:
  BinomialTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {}, {0, 1, 3, 8},
                            {}, {-1, -5, -7}, {0.0, 0.1, 0.7, 0.99}, {0},
                            {-0.1, 1.2}, {-1, 2}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& N, const T2& theta, const T3&,
                        T_rng& rng) const {
    return stan::math::binomial_rng(N, theta, rng);
  }

  template <typename T1>
  double pmf(int y, T1 N, double theta, double) const {
    if (y <= N) {
      return std::exp(stan::math::binomial_lpmf(y, N, theta));
    } else {
      return 0.0;
    }
  }
};

TEST(ProbDistributionsBinomial, errorCheck) {
  check_dist_throws_int_first_argument(BinomialTestRig());
}

TEST(ProbDistributionsBinomial, distributionCheck) {
  check_counts_int_real(BinomialTestRig());
}

TEST(ProbDistributionBinomiali, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::binomial_rng(4, 0.6, rng));
  EXPECT_THROW(stan::math::binomial_rng(-4, 0.6, rng), std::domain_error);
  EXPECT_THROW(stan::math::binomial_rng(4, -0.6, rng), std::domain_error);
  EXPECT_THROW(stan::math::binomial_rng(4, 2.6, rng), std::domain_error);
  EXPECT_THROW(
      stan::math::binomial_rng(4, stan::math::positive_infinity(), rng),
      std::domain_error);
}

TEST(ProbDistributionsBinomial, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::binomial_distribution<> dist(100, 0.6);
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 2));

  while (count < N) {
    int a = stan::math::binomial_rng(100, 0.6, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_REMROUND_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_REMROUND_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
      @brief  rem generic tag

      Represents the remround function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct remround_ : ext::elementwise_<remround_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<remround_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_remround_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::remround_, Site> dispatching_remround_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::remround_, Site>();
   }
   template<class... Args>
   struct impl_remround_;
  }
  /*!
    Computes the remainder of division.
    The return value is a0-n*a1, where n is the value a0/a1,
    rounded toward infinity.

    @par semantic:
    For any given value @c x, @c y of type @c T:

    @code
    T r = remround(x, y);
    @endcode

    For floating point values the code is equivalent to:

    @code
    T r = x-divround(x, y)*y;
    @endcode

    @param  a0
    @param  a1

    @return      a value of the same type as the input.

  **/
    BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::remround_, remround, 2)
} }

#endif


//==============================================================================
//          Copyright 2014    Jean-Thierry Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_CUMMIN_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_CUMMIN_HPP_INCLUDED

#include <boost/simd/swar/functions/cummin.hpp>
#include <boost/simd/include/functions/simd/insert.hpp>
#include <boost/simd/include/functions/scalar/min.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/swar/include/functions/shuffle.hpp>
#include <boost/simd/swar/functions/details/cum_helper.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT ( cummin_, tag::cpu_
                           , (A0)(X)
                           , ((simd_<arithmetic_<A0>,X>))
                           )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return helper<meta::cardinal_of<A0>::value>::unroll(a0);
    }

    private:
    template<std::size_t CARD, std::size_t PASS = CARD>
    struct helper
    {
      BOOST_FORCEINLINE static A0 unroll(const A0 & a0)
      {
        A0 tmp =  helper<CARD, PASS/2>::unroll(a0);
        return min(tmp,  shuffle<details::cum_helper<CARD, PASS>>(tmp));
      }
    };

    template<std::size_t CARD> struct helper<CARD, 0>
    {
      BOOST_FORCEINLINE static A0 unroll(const A0 & a0)
      {
        return a0;
      }
    };

  };
} } }

#endif


/* Functions-1.hpp (exercise 1.5.1)
Description:
	* Define functions used in Problem 5 Exercise 1.
Functions:
	*void printPerson(const Person &person_in): print the elements of the "Person" tuple { Name, Address, DateOfBirth }.
	*void SortPersons<unsigned propertyIndex>(vector<Person> &persons_in, bool descending): sort the passed Person vector based upon some property (0 : Name, 1 : Address, 2 : DateOfBirth), in descending order if possible
*/

#ifndef FUNCTIONS1_HPP
#define FUNCTIONS1_HPP
#ifndef _SILENCE_ALL_CXX17_DEPRECATION_WARNINGS
	#define _SILENCE_ALL_CXX17_DEPRECATION_WARNINGS
#endif

#include <boost\date_time.hpp>
#include <algorithm>
#include <iostream>
#include <string>
#include <tuple>
#include <vector>
#include "Exception.hpp"
#include "OutOfBounds.hpp"

/* 1.5.1 */
// a)
using Person = std::tuple <std::string, std::string, boost::gregorian::date>;
// b) Create function to print elements of Person
void printPerson(const Person &person_in) noexcept
{
	std::cout << "{ ";
	std::cout << "Name: " << std::get<0>(person_in);
	std::cout << ", Address: " << std::get<1>(person_in);
	std::cout << ", Date of Birth: " << std::get<2>(person_in);
	std::cout << " } " << std::endl;
}

// Problem 1: d) Create a function to sort the elements of a list of Persons, depending upon specified characteristic (name, address or date of birth):
template<unsigned propertyIndex>
void SortPersons(std::vector<Person> &persons_in, bool descending)
{
	if (propertyIndex > Person::_Mysize)
	{
		throw Exceptions::OutOfBounds("SortPersons() Error: Out of bounds.");
	}
	bool changed = true;
	unsigned size = persons_in.size();
	Person copy;
	// Sort the Person vector using bubblesort algorithm:
	while (changed)
	{
		changed = false;
		for (unsigned i = 0; i < size; i++)
		{
			for (unsigned j = 0; j < size - i - 1; j++)
			{
				// Get the current element:
				auto first = std::get<propertyIndex>(persons_in[j]);
				auto second = std::get<propertyIndex>(persons_in[j + 1]);
				// Swap the elements if out of order (depending upon ordering preference):
				if (descending && first < second)
				{
					changed = true;
					copy = std::move(persons_in.at(j));
					persons_in.at(j) = std::move(persons_in.at(j + 1));
					persons_in.at(j + 1) = copy;
				}
				else if (!descending && first > second)
				{
					changed = true;
					copy = std::move(persons_in.at(j));
					persons_in.at(j) = std::move(persons_in.at(j + 1));
					persons_in.at(j + 1) = copy;
				}
			}
		}
	}
}

#endif

Advanced C++ Course/Benjamin Rutan HW 1 Submission/1.5/1.5/Functions-1.hpp


// -*- mode: c++; -*-

#include <iostream>
using namespace std;

#include <lbp/frame_range.hpp>
#include <lbp/olbp.hpp>
#include <lbp/varlbp.hpp>
#include <lbp/utils.hpp>

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/functional/compose.hpp>
namespace hana = boost::hana;
using namespace hana::literals;

#include <opencv2/highgui.hpp>
using namespace cv;

#include <options.hpp>
#include <run.hpp>

auto bgr2gray = [](const cv::Mat& src) {
    return lbp::bgr2gray (src);
};

auto gray2float = [](const cv::Mat& src) {
    return lbp::gray2float (src);
};

auto equalize = [](const cv::Mat& src) {
    return lbp::equalize (src);
};

static void
f (cv::VideoCapture& cap, const options_t& opts) {
    const bool display = opts.have ("display");

    auto olbp = lbp::olbp< unsigned char, 2, 10 >;
    auto varlbp = lbp::varlbp< float, 1, 8 >;

    auto op = hana::compose (
        equalize, varlbp, gray2float, equalize, olbp, bgr2gray);

    for (auto& frame : lbp::getframes_from (cap)) {
        lbp::frame_delay temp { 0 };

        const auto dst = op (frame);

        if (display) {
            imshow ("Extended LBP (E-LBP) operator", dst);
        }

        if (temp.wait_for_key (27))
            break;
    }
}

int main (int argc, char** argv) {
    program_options_from (argc, argv);
    return run_with (f, global_options ()), 0;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/minimal.hpp>

#include <boost/units/base_dimension.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/scaled_base_unit.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/unit.hpp>

struct dimension1_tag : boost::units::base_dimension<dimension1_tag, 1> {};

typedef dimension1_tag::dimension_type dimension1;

struct dimension2_tag : boost::units::base_dimension<dimension2_tag, 2> {};

typedef dimension2_tag::dimension_type dimension2;

typedef boost::mpl::times<dimension1, dimension2>::type dimension12;

struct unit1_tag : boost::units::base_unit<unit1_tag, dimension1, 1> {};

struct unit2_tag : boost::units::base_unit<unit2_tag, dimension1, 2> {};

struct unit3_tag : boost::units::base_unit<unit3_tag, dimension1, 3> {};

struct unit4_tag : boost::units::base_unit<unit4_tag, dimension2, 4> {};

struct unit5_tag : boost::units::base_unit<unit5_tag, dimension12, 5> {};

BOOST_UNITS_DEFINE_CONVERSION_FACTOR(unit1_tag, unit2_tag, double, 2.0);

BOOST_UNITS_DEFINE_CONVERSION_FACTOR(unit2_tag, unit3_tag, double, 3.0);

typedef boost::units::multiply_typeof_helper<unit3_tag::unit_type, unit4_tag::unit_type>::type unit34_type;
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(unit5_tag, unit34_type , double, 5.0);

BOOST_UNITS_DEFAULT_CONVERSION(unit1_tag, unit2_tag::unit_type);

BOOST_UNITS_DEFAULT_CONVERSION(unit3_tag, unit2_tag::unit_type);

BOOST_UNITS_DEFAULT_CONVERSION(unit5_tag, unit34_type);

int test_main(int, char*[]) {
    double value1 = boost::units::conversion_factor(unit3_tag::unit_type(), unit1_tag::unit_type());
    BOOST_CHECK(std::abs(value1 - 1.0/6.0) < .0000000001);
    double value2 = boost::units::conversion_factor(unit5_tag::unit_type() / unit4_tag::unit_type(), unit1_tag::unit_type());
    BOOST_CHECK(std::abs(value2 - 5.0/6.0) < .0000000001);
    typedef boost::units::scaled_base_unit<unit5_tag, boost::units::scale<2, boost::units::static_rational<1> > > scaled_unit5_tag;
    double value3 = boost::units::conversion_factor(scaled_unit5_tag::unit_type() / unit4_tag::unit_type(), unit1_tag::unit_type());
    BOOST_CHECK(std::abs(value3 - 10.0/6.0) < .0000000001);
    return(0);
}


/**
 *
 */
#include <stdio.h>
#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>

#include "random_discriminant.hpp"
#include "data_item.hpp"
#include "pca.h"

#if 0
const char* TRAIN_FILE_NAME = "../DataSets/zip.data/zip.train";
const char* TEST_FILE_NAME = "../DataSets/zip.data/zip.test";
#else
const char* TRAIN_FILE_NAME =
  "C:/Users/Terence/Documents/GitHub/STAT775/DataSets/zip.data/zip.train";
const char* TEST_FILE_NAME =
  "C:/Users/Terence/Documents/GitHub/STAT775/DataSets/zip.data/zip.test";
#endif // if for environment

//LabeledData ReadDataFromFile(const char* file_name, const int dimensionality) {
///**
// *
// */
//  FILE* file = fopen(file_name, "r");
//  if (file == NULL) {
//    printf("Failed to read from file: %s\n", file_name);
//    exit(0);
//  }
//
//  std::vector<int> labels;
//  std::vector<std::vector<double> > data;
//
//  double dummy;
//  int read_result = fscanf(file, "%lf", &dummy);
//  while (read_result >= 1) {
//    labels.push_back((int) dummy);
//
//    std::vector<double> temp;
//    unsigned i;
//    for (i = 0; i < dimensionality; ++i) {
//      fscanf(file, " %lf", &dummy);
//      temp.push_back(dummy);
//    }
//    data.push_back(temp);
//
//    read_result = fscanf(file, " %lf", &dummy);
//  }
//
//  LabeledData labeledData = {labels, StlVectorsToEigenMatrix(data)};
//
//  return labeledData;
//}

int main(int argc, char** argv) {
   printf("%s\n%s\n", TRAIN_FILE_NAME, TEST_FILE_NAME);

  int i = 0;

  LabeledData data =
    ReadDataFromFile(TRAIN_FILE_NAME);

  ConstructPcaModel(data.data);

  //std::cout << data.data.row(0).head(10);

  //Eigen::MatrixXd test_0(2, 2); test_0 << -2.0, -1.0,
  //                                        -2.0, -1.0;
  //Eigen::MatrixXd test_1(2, 2); test_1 <<  1.0,  1.0,
  //                                         1.0,  1.0;

  //DiscriminantModel discriminant_model;
  //InitDiscriminantModel(&discriminant_model, 2, &test_0, &test_1);
  //PrintDiscriminantModel(&discriminant_model, stdout, true);
  //std::cout << "Class 0 predictions:\n" << GetDiscriminantPredictions(&discriminant_model, &test_0) << std::endl;
  //std::cout << "Class 1 predictions:\n" << GetDiscriminantPredictions(&discriminant_model, &test_1) << std::endl;

  //discriminant_model.projection_line << 0, 1;
  //std::cout << ((discriminant_model.projection_line).dot(test_0.row(0))) << std::endl;

  //LabeledData train = ReadDataFromFile(TRAIN_FILE_NAME, 256);
  //std::cout << train.data.row(0) << std::endl;

  //std::cout << train.labels[0] << ' ' << train.labels[1] << ' ' << train.labels[2];

  return 0;
}


///
/// @file dijkstra-bgl.h
/// @author Jxtopher
/// @version 1
/// @date 2019
/// @brief Implementation dijkstra with Boost Graph Library
/// \details BGL : https://www.boost.org/doc/libs/1_46_1/libs/graph/doc/adjacency_list.html
///          Dijkstra :https://fr.wikipedia.org/wiki/Algorithme_de_Dijkstra
///

#include <cstdlib>
#include <iostream>

#include <boost/graph/graph_utility.hpp> // print_graph
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>

using namespace std;
using namespace boost;

//-----------------------------------------------------------------------------
// Définitions des types pour le graphe
//-----------------------------------------------------------------------------

///
/// @brief Définition du type pour les noeuds graphe
///
struct VertexProperties {
    float d;
    int predecessor;
    VertexProperties() : d(std::numeric_limits<float>::infinity()), predecessor(-1) {}
    VertexProperties(float d, int predecessor) : d(d), predecessor(-1) {}
};

///
/// @brief Définition du type pour les liens graphe
///
struct EdgeProperties {
    int weight;
    EdgeProperties() : weight(0) { }
    EdgeProperties(int weight) : weight(weight) { }
};

struct EdgeInfoPropertyTag {
    typedef edge_property_tag kind;
    static std::size_t const num; // ???
};

std::size_t const EdgeInfoPropertyTag::num = (std::size_t)&EdgeInfoPropertyTag::num;
typedef property<EdgeInfoPropertyTag, EdgeProperties> edge_info_prop_type;

///
/// @brief Type de graphe
///
typedef adjacency_list<
    boost::vecS, boost::vecS, boost::undirectedS,
    VertexProperties,                               // Type vertex
    edge_info_prop_type                             // Type edge
> Graph;

//-----------------------------------------------------------------------------
// En plus
//-----------------------------------------------------------------------------
void test(Graph &g) {
    // Visiter les voisins du sommet 0
    auto neighbours = boost::adjacent_vertices(0, g);

    for (auto vd : make_iterator_range(neighbours))
        std::cout << "0 has adjacent vertex " << vd << "\n";

    g[0].d = 42;
    cout<<g[0].d<<endl;

    //cout<<edge(0,2,g).second<<endl;

    EdgeProperties p = get(EdgeInfoPropertyTag(), g, edge(0,2,g).first);
    std::cout << "weight: " << p.weight << std::endl;

    get(EdgeInfoPropertyTag(), g, edge(0,2,g).first).weight = 33;
    std::cout << "weight: " << get(EdgeInfoPropertyTag(), g, edge(0,2,g).first).weight << std::endl;

    // Nombre de sommets
    cout<<num_vertices(g)<<endl;
    cout<<num_edges(g)<<endl;

    // Affiche le graphe
    print_graph(g);

}
//-----------------------------------------------------------------------------
// dijkstra
//-----------------------------------------------------------------------------
///
/// @brief Crée une instance de graphe
///
/// \param g : graphe
///
void createInstanceGraph(Graph &g) {
    add_vertex(VertexProperties(std::numeric_limits<float>::infinity(), -1), g);
    add_vertex(g);
    add_vertex(g);
    add_vertex(g);
    add_vertex(g);

    add_edge(0, 1, EdgeProperties(7), g);
    add_edge(0, 2, EdgeProperties(9), g);
    add_edge(0, 5, EdgeProperties(14), g);
    add_edge(1, 3, EdgeProperties(15), g);
    add_edge(1, 2, EdgeProperties(10), g);
    add_edge(2, 3, EdgeProperties(11), g);
    add_edge(2, 5, EdgeProperties(2), g);
    add_edge(4, 3, EdgeProperties(6), g);
    add_edge(5, 4, EdgeProperties(9), g);
}

///
/// @brief donne le plus petit élément de la liste l
///
/// \param g : graphe
/// \param l : liste de noeud
///
/// \return le plus petit élément de la liste l
///
unsigned int extrait_min(Graph const &g, list<unsigned int> &l) {
    list<unsigned int>::iterator iter = min_element(l.begin(), l.end());
    l.erase(iter);
    return *iter;
}

///
/// @brief *
///
/// \param u : *
/// \param v : *
///
void relacher(Graph &g, unsigned int u, unsigned int v) {
    EdgeProperties p = get(EdgeInfoPropertyTag(), g, edge(u, v, g).first);
    //std::cout << "weight: " << p.weight << std::endl;

    if ((g[u].d + p.weight) < g[v].d ) {
        g[v].d = g[u].d + p.weight;
        g[v].predecessor = u;
    }
}

///
/// @brief algo de dijkstra
///
/// \param g : graphe
///
/// \return le plus court chemin
///
list<unsigned int> dijkstra(Graph &g) {
    g[0].d = 0;             // Sommet de début
    list<unsigned int> F;   // liste des sommets à visiter
    list<unsigned int> E;   // liste des sommets avec longueurs finales de plus court chemin à partir de l’origine s
    for (unsigned int i = 0 ; i < num_vertices(g) ; i++)
        F.push_back(i);

    while (!F.empty()) {
        unsigned int u = extrait_min(g, F);
        E.push_back(u);

        // Visite les voisins du sommet u
        auto neighbours = boost::adjacent_vertices(u, g);
        for (auto v : make_iterator_range(neighbours)) {
            //std::cout << "0 has adjacent vertex " << v << "\n";
            relacher(g, u, v);
        }
    }

    // Lecture du résultat
    list<unsigned int> resultat;
    int x = num_vertices(g) - 1;
    resultat.push_back(x);
    while (0 < x) {
        x = g[x].predecessor;
        resultat.push_back(x);
    }

    return resultat;
}

int main() {
    // Crée un graphe g
    Graph g;

    // Crée une instance de graphe
    createInstanceGraph(g);

    // Affiche le graphe
    cout<<"[+] liste d'adjacence :"<<endl;
    print_graph(g);

    // Appel de l'algo dijkstra
    list<unsigned int> r = dijkstra(g);

    // Affiche le plus court chemin
    cout<<endl<<"[+] le plus court chemin :"<<endl;
    for (auto v : r)
        std::cout << v<<" ";
    cout<<endl;

    return EXIT_SUCCESS;
}


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file ErrorTermWheelTest.cpp
    \brief This file tests the ErrorTermWheel class.
  */

#include <Eigen/Core>

#include <boost/make_shared.hpp>

#include <gtest/gtest.h>

#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <aslam/backend/EuclideanPoint.hpp>

#include <aslam/calibration/data-structures/VectorDesignVariable.h>

#include "aslam/calibration/car/error-terms/ErrorTermSteering.h"

using namespace aslam::backend;
using namespace aslam::calibration;

TEST(AslamCalibrationTestSuite, testErrorTermSteering) {

  // linear velocity
  const Eigen::Matrix<double, 3, 1> v(1.5, 1.6, 2.6);
  auto v_dv = boost::make_shared<EuclideanPoint>(v);
  EuclideanExpression v_exp(v_dv);

  // steering coefficients
  VectorDesignVariable<4> a((VectorDesignVariable<4>::Container() << 0.1, 1.5,
    0.2, 0.5).finished());

  // error term steering
  ErrorTermSteering e_st(v_exp, 1.5, 1, &a);

  // test the error terms
  try {
    ErrorTermTestHarness<1> harness(&e_st);
    harness.testAll();
  }
  catch (const std::exception& e) {
    FAIL() << e.what();
  }

  // test accessors
  e_st.setVariance(10);
  ASSERT_EQ(e_st.getVariance(), 10);
  e_st.setMeasurement(10);
  ASSERT_EQ(e_st.getMeasurement(), 10);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/include/constants/true_false.hpp>
#include <boost/simd/include/constants/allbits.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include "../constant.hpp"

NT2_TEST_CASE_TPL( true_false, BOOST_SIMD_SIMD_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<boost::simd::logical<T>,ext_t>  n_t;

  NT2_CHECK_CONSTANT(False, boost::simd::Zero<T>() , n_t );
  NT2_CHECK_CONSTANT(True , boost::simd::Allbits<T>()  , n_t );
}


/*! \file solveeommain.cpp
    \brief 単振り子に対して運動方程式を解く関数群の実装

    Copyright ©  2016 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/
#include "solveeommain.h"
#include <boost/utility/in_place_factory.hpp>   // boost::in_place

extern "C" {
    float __stdcall gettheta1()
    {
        return pse->Theta1();
    }

    float __stdcall gettheta2()
    {
        return pse->Theta2();
    }

    float __stdcall getv1()
    {
        return pse->V1();
    }

    float __stdcall getv2()
    {
        return pse->V2();
    }

    void __stdcall init(float l, float m, float theta1_0, float theta2_0)
    {
        pse = boost::in_place(l, m, theta1_0, theta2_0);
    }

    float __stdcall kinetic_energy()
    {
        return pse->kinetic_energy();
    }

    void __stdcall nextstep(float dt, float * theta1, float * theta2)
    {
        (*pse)(dt, theta1, theta2);
    }

    float __stdcall potential_energy()
    {
        return pse->potential_energy();
    }

    void __stdcall saveresult(double dt, std::string const & filename, double t)
    {
        (*pse)(dt, filename, t);
    }

    void __stdcall settheta1(float theta)
    {
        pse->Theta1(theta);
    }

    void __stdcall settheta2(float theta)
    {
        pse->Theta2(theta);
    }

    void __stdcall setv1(float v)
    {
        pse->V1(v);
    }

    void __stdcall setv2(float v)
    {
        pse->V2(v);
    }
}


/***************************************************************************
* Copyright (c) Johan Mabille, Sylvain Corlay and Wolf Vollprecht          *
* Copyright (c) QuantStack                                                 *
*                                                                          *
* Distributed under the terms of the BSD 3-Clause License.                 *
*                                                                          *
* The full license is in the file LICENSE, distributed with this software. *
****************************************************************************/

#include <benchmark/benchmark.h>

#ifdef HAS_XTENSOR
#include "xtensor/xnoalias.hpp"
#include "xtensor/xio.hpp"
#include "xtensor/xrandom.hpp"
#include "xtensor/xtensor.hpp"
#include "xtensor/xarray.hpp"
#endif

#ifdef HAS_EIGEN
#include <Eigen/Dense>
#include <Eigen/Core>
#endif

#ifdef HAS_BLITZ
#include <blitz/array.h>
#endif

#define RANGE 3, 1000
#define MULTIPLIER 8

#ifdef HAS_XTENSOR
void IterateWhole2D_XTensor(benchmark::State& state)
{
    xt::xtensor<double, 2> vTensor({state.range(0), state.range(0)});
    for (auto _ : state)
    {
        double vTmp = 0.0;
        for (auto it = vTensor.begin(); it != vTensor.end(); ++it) {
            vTmp += *it;
        }
        benchmark::DoNotOptimize(vTmp);
    }
}
BENCHMARK(IterateWhole2D_XTensor)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

//#ifdef HAS_EIGEN
//void IterateWhole2D_Eigen(benchmark::State& state)
//{
//    using namespace Eigen;
//    MatrixXd vMatrix(state.range(0), state.range(0));
//    for (auto _ : state)
//    {
//        double vTmp = 0.0;
//        for (auto it = vMatrix.begin(); it != vMatrix.end(); ++it) {
//            vTmp += *it;
//        }
//        benchmark::DoNotOptimize(vTmp);
//    }
//}
//BENCHMARK(IterateWhole2D_Eigen)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
//#endif

#ifdef HAS_BLITZ
void IterateWhole2D_Blitz(benchmark::State& state)
{
    using namespace blitz;
    Array<double, 2> vArray(state.range(0), state.range(0));
    for (auto _ : state)
    {
        double vTmp = 0.0;
        for (auto it = vArray.begin(); it != vArray.end(); ++it) {
            vTmp += *it;
        }
        benchmark::DoNotOptimize(vTmp);
    }
}
BENCHMARK(IterateWhole2D_Blitz)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#undef RANGE
#undef MULTIPLIER


// Copyright (C) 2019 Kirill Okhotnikov.
//
// Use, modification, and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
//
// You are welcome to contact the author at:
//  kirill.okhotnikov@gmail.com
//

//#define SAFE_REDUCED_LIST
#include "types_list.hpp"

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

#include <cstdint>
#include <iostream>
#include <limits>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/number.hpp>
#include "safe/backends/native/safe_compare.hpp"

void check_compare(const precision_type_t &v1, const precision_type_t &v2) {
  using namespace boost;
  mpl::for_each<types_arithmetic_t>([&v1, &v2](const auto u) -> void {
    typedef typename std::remove_cv<decltype(u)>::type U;
    if ( fits_in<U>(v1) && precision_saved<U>(v1, v2) ) {
      mpl::for_each<types_arithmetic_t>([&v1, &v2](const auto r) -> void {
        typedef typename std::remove_cv<decltype(r)>::type R;
        if ( fits_in<R>(v2) && precision_saved<R>(v2, v1) ) {
          U m1 = static_cast<U>(v1);
          R m2 = static_cast<R>(v2);
          //std::cout << m1 << " - " << m2 << ": " << v1 << " - " << v2 << std::endl;
          using compare_t = safe_compare<U, R>;
          BOOST_CHECK_EQUAL(compare_t::less(m1, m2), v1 < v2);
          BOOST_CHECK_EQUAL(compare_t::greater(m1, m2), v1 > v2);
          BOOST_CHECK_EQUAL(compare_t::leq(m1, m2), v1 <= v2);
          BOOST_CHECK_EQUAL(compare_t::geq(m1, m2), v1 >= v2);
          BOOST_CHECK_EQUAL(compare_t::eq(m1, m2), v1 == v2);
          BOOST_CHECK_EQUAL(compare_t::neq(m1, m2), v1 != v2);
          if (    std::is_signed<U>::value == std::is_signed<R>::value
               || ( m1 >= 0 && m2 >= 0 ) ) {
            BOOST_CHECK_EQUAL(m1 < m2, v1 < v2);
            BOOST_CHECK_EQUAL(m1 > m2, v1 > v2);
            BOOST_CHECK_EQUAL(m1 <= m2, v1 <= v2);
            BOOST_CHECK_EQUAL(m1 >= m2, v1 >= v2);
            BOOST_CHECK_EQUAL(m1 == m2, v1 == v2);
            BOOST_CHECK_EQUAL(m1 != m2, v1 != v2);
          }
        }
      });
    }
  });

}

BOOST_AUTO_TEST_CASE( compare_check )
{
  auto checks = values_to_check();
  for(const auto &v1 : checks) {
    for(const auto &v2: checks) {
      check_compare(v1, v2);
    }
  }
}


/*

 * @file LinearInterpolationVectorSpaceCurve.hpp
 * @date Aug 17, 2014
 * @author Paul Furgale, Abel Gawel, Renaud Dube

#include <curves/LinearInterpolationVectorSpaceCurve.hpp>
#include <iostream>
#include <boost/bind.hpp>
#include <boost/function.hpp>

namespace curves {

template<int N>
LinearInterpolationVectorSpaceCurve<N>::LinearInterpolationVectorSpaceCurve() : VectorSpaceCurve<N>() {}

template<int N>
LinearInterpolationVectorSpaceCurve<N>::~LinearInterpolationVectorSpaceCurve() {}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::print(const std::string& str) const {
  std::cout << "=========================================" << std::endl;
  std::cout << "=======LINEAR INTERPOLATION CURVE========" << std::endl;
  std::cout << str << std::endl;
  std::cout << "number of coefficients: " << manager_.size() << std::endl;
  std::cout << "dimension: " << N << std::endl;
  std::stringstream ss;
  std::vector<Key> keys;
  std::vector<Time> times;
  manager_.getTimes(&times);
  manager_.getKeys(&keys);
  std::cout << "curve defined between times: " << manager_.getMinTime() <<
      " and " << manager_.getMaxTime() <<std::endl;
  std::cout <<"=========================================" <<std::endl;
  for (size_t i = 0; i < manager_.size(); i++) {
    ss << "coefficient " << keys[i] << ": ";
    std::cout << ss.str() << manager_.getCoefficientByKey(keys[i]).transpose() << std::endl;
    std::cout << " | time: " << times[i];
    std::cout << std::endl;
    ss.str("");
  }
  std::cout <<"=========================================" <<std::endl;
}

template<int N>
Time LinearInterpolationVectorSpaceCurve<N>::getMaxTime() const {
  return manager_.getMaxTime();
}

template<int N>
Time LinearInterpolationVectorSpaceCurve<N>::getMinTime() const {
  return manager_.getMinTime();
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::fitCurve(const std::vector<Time>& times,
                                                      const std::vector<ValueType>& values,
                                                      std::vector<Key>* outKeys) {
  CHECK_EQ(times.size(), values.size());

  if(times.size() > 0) {
    manager_.clear();
    if (outKeys != NULL) {
      outKeys->clear();
      outKeys->reserve(times.size());
    }
    std::vector<Coefficient> coefficients;
    coefficients.reserve(times.size());
    size_t vsize = values[0].size();
    for(size_t i = 0; i < values.size(); ++i) {
      CHECK_EQ(vsize, values[i].size()) << "The vectors must be uniform length.";
      coefficients.push_back(Coefficient(values[i]));
    }
    manager_.insertCoefficients(times,coefficients,outKeys);
  }
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::extend(const std::vector<Time>& times,
                                                    const std::vector<ValueType>& values,
                                                    std::vector<Key>* outKeys) {

  CHECK_EQ(times.size(), values.size()) << "number of times and number of coefficients don't match";
  std::vector<Coefficient> coefficients(values.size());
  for (size_t i = 0; i < values.size(); ++i) {
    coefficients[i] = Coefficient(values[i]);
  }
  manager_.insertCoefficients(times, coefficients, outKeys);
}

template<int N>
typename LinearInterpolationVectorSpaceCurve<N>::ValueType
LinearInterpolationVectorSpaceCurve<N>::evaluate(Time time) const {
  CoefficientIter rval0, rval1;
  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
  CHECK(success) << "Unable to get the coefficients at time " << time;

  Time dt = rval1->first - rval0->first;
  Time t = rval1->first - time;
  // Alpha goes from zero to one.
  double alpha = double(t)/double(dt);

  return alpha * rval0->second.coefficient + (1.0 - alpha) * rval1->second.coefficient;
}

template<int N>
typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType
LinearInterpolationVectorSpaceCurve<N>::evaluateDerivative(Time time,
                                                           unsigned derivativeOrder) const {

  // time is out of bound --> error
  CHECK_GE(time, this->getMinTime()) << "Time out of bounds";
  CHECK_LT(time, this->getMaxTime()) << "Time out of bounds";
  CHECK_GT(derivativeOrder, 0) << "DerivativeOrder must be greater than 0";

  typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType dCoeff;
  Time dt;
  CoefficientIter rval0, rval1;
  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
  CHECK(success) << "Unable to get the coefficients at time " << time;
  // first derivative
  if (derivativeOrder == 1) {
    dCoeff = rval1->second.coefficient - rval0->second.coefficient;
    dt = rval1->first - rval0->first;
    return dCoeff/dt;
  } else { // order of derivative > 1 returns vector of zeros
    dCoeff.Zero();
    return dCoeff;
  }
}

// Evaluation function in functional form. To be passed to the expression
template<int N>
Eigen::Matrix<double,N,1> linearInterpolation(Eigen::Matrix<double,N,1>  v1,
                                              Eigen::Matrix<double,N,1>  v2, double alpha,
                                              gtsam::OptionalJacobian<N,N> H1,
                                              gtsam::OptionalJacobian<N,N> H2) {
  if (H1) { *H1 = Eigen::Matrix<double,N,N>::Identity()*(1-alpha); }
  if (H2) { *H2 = Eigen::Matrix<double,N,N>::Identity()*alpha; }

  return v1*(1-alpha) + v2*alpha;
}

template<int N>
gtsam::Expression<typename LinearInterpolationVectorSpaceCurve<N>::ValueType>
LinearInterpolationVectorSpaceCurve<N>::getValueExpression(const Time& time) const {
  typedef typename LinearInterpolationVectorSpaceCurve<N>::ValueType ValueType;
  using namespace gtsam;
  CoefficientIter rval0, rval1;
  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
  CHECK(success) << "Unable to get the coefficients at time " << time;

  Expression<ValueType> leaf1(rval0->second.key);
  Expression<ValueType> leaf2(rval1->second.key);

  double alpha = double(time - rval0->first)/double(rval1->first - rval0->first);

  Expression<ValueType> rval(boost::bind(&linearInterpolation<N>, _1, _2, alpha, _3, _4),
                             leaf1, leaf2);

  return rval;
}

template<int N>
gtsam::Expression<typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType>
LinearInterpolationVectorSpaceCurve<N>::getDerivativeExpression(const Time& time, unsigned derivativeOrder) const {
  // \todo Abel and Renaud
  CHECK(false) << "Not implemented";
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values* values) const {
  manager_.initializeGTSAMValues(keys, values);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::initializeGTSAMValues(gtsam::Values* values) const {
  manager_.initializeGTSAMValues(values);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::updateFromGTSAMValues(const gtsam::Values& values) {
  manager_.updateFromGTSAMValues(values);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::clear() {
  manager_.clear();
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::addPriorFactors(gtsam::NonlinearFactorGraph* graph, Time priorTime) const {
  gtsam::noiseModel::Constrained::shared_ptr priorNoise = gtsam::noiseModel::Constrained::All(gtsam::traits<Coefficient>::dimension);

  // Constraint the coefficients which influence the curve value at priorTime
  CoefficientIter rVal0, rVal1;
  manager_.getCoefficientsAt(priorTime, &rVal0, &rVal1);

  gtsam::ExpressionFactor<Coefficient> factor0(priorNoise,
                                          rVal0->second.coefficient,
                                          gtsam::Expression<Coefficient>(rVal0->second.key));
  gtsam::ExpressionFactor<Coefficient> factor1(priorNoise,
                                          rVal1->second.coefficient,
                                          gtsam::Expression<Coefficient>(rVal1->second.key));
  graph->push_back(factor0);
  graph->push_back(factor1);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::transformCurve(const ValueType T) {
  //todo
}

template<int N>
Time LinearInterpolationVectorSpaceCurve<N>::getTimeAtKey(gtsam::Key key) const {
  return manager_.getCoefficientTimeByKey(key);
}

} // namespace curves
*/


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 trigonometric toolbox - rem_pio2/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of trigonometric components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 11/02/2011
///
#include <nt2/trigonometric/include/functions/rem_pio2.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/trigonometric/constants.hpp>
#include <nt2/constant/constant.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <nt2/sdk/meta/as_integer.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

NT2_TEST_CASE_TPL ( rem_pio2_real__1_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::rem_pio2;
  using nt2::tag::rem_pio2_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::call<rem_pio2_(vT)>::type r_t;

  // specific values tests
  typedef typename boost::fusion::result_of::value_at_c<r_t,0>::type r_t0;
  typedef typename boost::fusion::result_of::value_at_c<r_t,1>::type r_t1;
  typedef typename boost::fusion::result_of::value_at_c<r_t,2>::type r_t2;
  {
    r_t res = rem_pio2(nt2::Zero<vT>());
    NT2_TEST_ULP_EQUAL( boost::fusion::get<0>(res)[0], nt2::Zero<r_t0>()[0], 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<1>(res)[0], nt2::Zero<r_t1>()[0], 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<2>(res)[0], nt2::Zero<r_t2>()[0], 0.5);
  }
} // end of test for floating_

NT2_TEST_CASE_TPL ( rem_pio2_targeted,  NT2_SIMD_REAL_TYPES)
{

  using nt2::rem_pio2;
  using nt2::tag::rem_pio2_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::as_integer<vT>::type ivT;

  ivT n;
  vT x = nt2::Pio_2<vT>(), xr;
  n = rem_pio2(x, xr, nt2::meta::as_<nt2::big_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 0.5);

  n = rem_pio2(x, xr, nt2::meta::as_<nt2::medium_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 0.5);

  n = rem_pio2(x, xr, nt2::meta::as_<nt2::small_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 0.5);

  n = rem_pio2(x, xr, nt2::meta::as_<nt2::very_small_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 0.5);

}


/*
 *  matrix_map_view_test_2.cpp
 *  MTL
 *
 *	Test mat::rscaled_view and mat::divide_by_view
 *
 *  Created by Hui Li (huil@Princeton.EDU)
 *
 */

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/matrix/hermitian_view.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/scale.hpp>
#include <boost/numeric/mtl/operation/rscale.hpp>
#include <boost/numeric/mtl/operation/divide_by.hpp>
#include <boost/numeric/mtl/operation/hermitian.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
#include <boost/numeric/mtl/operation/mult_result.hpp>
#include <boost/numeric/mtl/operation/mult.hpp>
#include <boost/numeric/mtl/operation/exp.hpp>
#include <boost/numeric/mtl/operation/iota.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>

using namespace std;

typedef complex<double> ct;

double value(double)
{
    return 7.0;
}

complex<double> value(complex<double>)
{
    return ct(7.0, 1.0);
}

// right scaled value
double rsvalue(double)
{
    return 14.0;
}

ct rsvalue(ct)
{
    return ct(14.0, 2.0);
}

// complex scaled value
ct crsvalue(double)
{
    return ct(0.0, 7.0);
}

ct crsvalue(ct)
{
    return ct(-1.0, 7.0);
}

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    set_to_zero(matrix);
    typedef typename Matrix::value_type value_type;
    value_type ref(0);

    {
		mtl::mat::inserter<Matrix>  ins(matrix);
		ins(2, 3) << value(ref);
		ins(4, 3) << value(ref) + 1.0;
		ins(2, 5) << value(ref) + 2.0;
    }

    cout << "\n\n" << name << "\n";
    cout << "Original matrix:\n" << matrix << "\n";

#if 0
    std::cout << "ashape double: " << typeid(typename ashape::ashape<double>::type).name() << "\n";
    std::cout << "ashape matrix: " << typeid(typename ashape::ashape<Matrix>::type).name() << "\n";
    typedef typename ashape::ashape<double>::type   dshape;
    typedef typename ashape::ashape<Matrix>::type   mshape;

    std::cout << "ashape mult_op: " << typeid(typename ashape::mult_op<dshape, mshape>::type()).name() << "\n";
    typedef typename ashape::mult_op<dshape, mshape>::type rshape;

    std::cout << "result type: " << typeid(typename traits::mult_result_aux<double, Matrix, rshape>::type).name() << "\n";
#endif

	// test rscaled_view
    mtl::mat::rscaled_view<Matrix, double>  rscaled_matrix(matrix, 2.0);
    cout << "matrix  right scaled with 2.0\n" << rscaled_matrix << "\n";
    MTL_THROW_IF(rscaled_matrix(2, 3) != rsvalue(ref), mtl::runtime_error("right scaling wrong"));

    cout << "matrix  right scaled with 2.0 (as operator)\n" << matrix * 2.0 << "\n";
    MTL_THROW_IF((matrix * 2.0)(2, 3) != rsvalue(ref), mtl::runtime_error("right scaling wrong"));

    mtl::mat::rscaled_view<Matrix, ct>  crscaled_matrix(matrix, ct(0.0, 1.0));
    cout << "matrix right scaled with i (complex(0, 1))\n" << crscaled_matrix << "\n";
    MTL_THROW_IF(crscaled_matrix(2, 3) != crsvalue(ref), mtl::runtime_error("complex right scaling wrong"));

    cout << "matrix right scaled with 2.0 (free function)\n" << rscale(matrix, 2.0) << "\n";
    MTL_THROW_IF(rscale(matrix, 2.0)(2, 3) != rsvalue(ref), mtl::runtime_error("scaling wrong"));

    cout << "matrix right scaled with i (complex(0, 1)) (free function)\n" << rscale(matrix, ct(0.0, 1.0)) << "\n";
    MTL_THROW_IF(rscale(matrix, ct(0.0, 1.0))(2, 3) != crsvalue(ref), mtl::runtime_error("complex right scaling wrong"));

	// test divide_by_view
    mtl::mat::divide_by_view<Matrix, double>  div_matrix(matrix, 0.5);
    cout << "matrix divide by 0.5\n" << div_matrix << "\n";
    MTL_THROW_IF(div_matrix(2, 3) != rsvalue(ref), mtl::runtime_error("divide_by wrong"));

    cout << "matrix divide by 0.5 (as operator)\n" << matrix / 0.5 << "\n";
    MTL_THROW_IF((matrix / 0.5)(2, 3) != rsvalue(ref), mtl::runtime_error("divide_by wrong"));

    mtl::mat::divide_by_view<Matrix, ct>  cdiv_matrix(matrix, ct(0.0, -1.0));
    cout << "matrix divide by -i (complex(0, -1))\n" << cdiv_matrix << "\n";
    MTL_THROW_IF(cdiv_matrix(2, 3) != crsvalue(ref), mtl::runtime_error("complex divide_by wrong"));

    cout << "matrix divide by 0.5 (free function)\n" << rscale(matrix, 0.5) << "\n";
    MTL_THROW_IF(divide_by(matrix, 0.5)(2, 3) != rsvalue(ref), mtl::runtime_error("scaling wrong"));

    cout << "matrix divide by -i (complex(0, 1)) (free function)\n" << divide_by(matrix, ct(0.0, -1.0)) << "\n";
    MTL_THROW_IF(divide_by(matrix, ct(0.0, -1.0))(2, 3) != crsvalue(ref), mtl::runtime_error("complex right scaling wrong"));

    mtl::mat::exp_view<Matrix>   exp_matrix(matrix);
    // cout << "exp matrix is\n" << exp_matrix << '\n';
    cout << "exp matrix is\n" << exp(matrix) << '\n';

    mtl::dense_vector<value_type> v(num_cols(matrix), value_type(1)), w;

    //  To be deleted some day
//     cout <<  typeid(typename mtl::sfunctor::exp<typename Matrix::value_type>::result_type).name() << '\n';
//     cout <<  typeid(typename mtl::mat::map_view<mtl::sfunctor::exp<typename Matrix::value_type>, Matrix>::value_type).name() << '\n';
//     cout <<  typeid(typename mtl::mat::exp_view<Matrix>::value_type).name() << '\n';
//     cout <<  typeid(typename mtl::Collection<mtl::mat::exp_view<Matrix> >::value_type).name() << '\n';
//     cout <<  typeid(typename mtl::Collection<mtl::mat::imag_view<Matrix> >::value_type).name() << '\n';

    w= exp(matrix) * v;
    cout << "exp(A) * vec(1) = " << w << '\n';
    // cout << "exp(A) * vec(1) = " << exp(matrix) * v << '\n'; // maybe later

}

int main(int argc, char* argv[])
{
    using namespace mtl;
    unsigned size= 7;
    if (argc > 1) size= atoi(argv[1]);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    return 0;
}


/*****************************************************************
 Author: J.-G. Young, <jean.gabriel.young@gmail.com>
******************************************************************
 This program creates a network using the complete structural
 preferential attachment model described in

  A) Hébert-Dufresne et al., Phys.  Rev. Lett. 107, 1, (2011).
  B) Hébert-Dufresne et al., Phys. Rev. E 85, 026108, (2012).
  C) Young et al., Phys. Rev. E (2016).

******************************************************************
 Usage:
******************************************************************
  Compilation:
    g++ -o3 -W -Wall -Wextra -pedantic -std=c++0x spa_main.cpp spa_network.cpp -lboost_program_options -o SPA

  Call:
    Run ./SPA -h to see the list of parameters.

*****************************************************************/

// Configuration file
#include "config.h"
// C libraries
#include <stdlib.h> // C error codes
// STL
#include <string>   // const char* to std::string()
#include <chrono>   // c++11, benchmarking
// boost
#if HAVE_LIBBOOST_PROGRAM_OPTIONS == 1
  #include <boost/program_options.hpp>
#endif
// Project files
#include "spa_network.hpp" // spa_network_t
#include "interface.hpp"  // boost_interface(3) and barbone_interface(3)
#include "output_functions.hpp"  // print_outputs(2)
#include "spa_algorithm.hpp" // spa_star(5) and spa_basic(4)
// Typdef and helpers
#include "types.hpp" // spa_parameters_t
#include "errors.hpp" // detailed error code
#include "includes/logger.hpp" // LOGGER macros

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

  LOGGER_START("")

  // Interface (parse argv, handle parameter errors, help message, etc.)

  spa_parameters_t prog_params;

  #if HAVE_LIBBOOST_PROGRAM_OPTIONS == 1
    int interface_return = boost_interface(argc, argv, prog_params);
  #else
    int interface_return = barebone_interface(argc, argv, prog_params);
  #endif

  switch(interface_return) {
    case EXIT_FAILURE :
      LOGGER_WRITE(Logger::DEBUG, "Exiting with interface error: invalid invocation of "+std::string(argv[0])+",")
      LOGGER_STOP()
      return EXIT_FAILURE;

    case FILE_ERROR :
      LOGGER_WRITE(Logger::ERROR, "Exiting with interface error: could not open the configuration file.")
      LOGGER_STOP()
      return EXIT_FAILURE;

    case ELEM_MISSING_ERROR :
      LOGGER_WRITE(Logger::ERROR, "Exiting with interface error: missing parameters.")
      LOGGER_STOP()
      return EXIT_FAILURE;

    case LOGIC_ERROR :
      LOGGER_WRITE(Logger::ERROR, "Exiting with interface error: parameter error.")
      LOGGER_STOP()
      return EXIT_FAILURE;

    case STOP_WITH_SUCCESS :
      LOGGER_WRITE(Logger::DEBUG, "Exiting with interface success: successfully parsed input (help message or template produced).")
      LOGGER_STOP()
      return EXIT_SUCCESS;

    default: break;
  }

  // Object declarations
  #if HAVE_STEADY_CLOCK == 1
    const auto simulation_begins = std::chrono::steady_clock::now();
  #else
    const auto simulation_begins = std::chrono::monotonic_clock::now();
  #endif

  LOGGER_WRITE(Logger::DEBUG, "Instantiation of the spa_network_t class.")
  spa_network_t network(prog_params.final_size.value, prog_params.basic_size.value);

  LOGGER_WRITE(Logger::DEBUG, "Instantiation of the random number generator engine "+VAR_PRINT(prog_params.seed.value)+".")
  std::mt19937 engine(prog_params.seed.value);

  // Setup and simulation
  if (prog_params.edge_list.initialized || prog_params.degrees.initialized || prog_params.internal_degrees.initialized || prog_params.projected_internal_degrees.initialized) {
    LOGGER_WRITE(Logger::INFO, "Connectivity information requested: complete simulation mode.")
    network.initialize(prog_params.initial_size.value);
    spa_star(prog_params.p.value, prog_params.q.value, prog_params.r.value, network, engine);
  }
  else if (prog_params.connected_edge_list.initialized || prog_params.connected_degrees.initialized) {
    LOGGER_WRITE(Logger::INFO, "Legacy connectivity information requested: legacy simulation mode.")
    network.set_handle_links_status(false);
    network.initialize(prog_params.initial_size.value);
    spa_basic(prog_params.p.value, prog_params.q.value, network, engine);
  }
  else {
    LOGGER_WRITE(Logger::INFO, "No connectivity information requested: legacy simulation mode.")
    network.set_handle_links_status(false);
    network.initialize(prog_params.initial_size.value);
    spa_basic(prog_params.p.value, prog_params.q.value, network, engine);
  }
  LOGGER_WRITE(Logger::INFO, "Simulation completed.")

  // Timer
  #if HAVE_STEADY_CLOCK == 1
    const auto simulation_ends = std::chrono::steady_clock::now();
  #else
    const auto simulation_ends = std::chrono::monotonic_clock::now();
  #endif
  double simulation_duration = std::chrono::duration_cast<std::chrono::milliseconds>(simulation_ends-simulation_begins).count()/1000.0;

  // Outputs
  LOGGER_WRITE(Logger::INFO, "Processing outputs.")
  process_and_output(network, prog_params, simulation_duration); // this alters the network.

  LOGGER_WRITE(Logger::INFO, "Exiting without error.")
  LOGGER_STOP()

  return EXIT_SUCCESS;
}


/* Computes the joint distribution over all the latent variables in the model using Gibbs Sampling.
 *
 * This latent variable model adapted from the one described in "Learning an Input Filter for
 * Argument Structure Acquisition" by Perkins, Feldman, and Lidz. See the paper for details.
 */

#include "metropolis_hastings.hpp"
#include "sample_models.hpp"
#include "utils.hpp"

#include <CLI/CLI.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/trivial.hpp>
#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <tuple>
#include <utility>

using namespace FilterModel;
namespace logging = boost::log;

/**
 * Jointly inferrs the alpha vector of each object, the epsilon, and the delta value for some
 * objects by using a fixed number of iterations of gibbs sampling.
 *
 * The sampling uses the following flow:
 *   alpha -> epsilon -> delta.
 *
 * This function takes as input a vector of category count vectors of each object and the number of
 * iterations or time steps to sample over.
 *
 * This function outputs a tuple of three vectors. The first vector contains the sampled
 * alpha of each object in the data at each time step. So the first element of outer vector
 * is a vector of alphas sampled at the first time step, the second element is the
 * alphas at the second time step, etc. The second and third vectors are the sampled epsilon
 * and delta values at each time step respectively.
 */
std::tuple<std::vector<std::vector<alpha_t>>, std::vector<double>, std::vector<delta_t>,
           std::vector<double>>
joint_inference(std::vector<category_counts_t> &data, int iterations,
                std::function<void(std::vector<std::vector<alpha_t>>, std::vector<double>,
                                   std::vector<delta_t>, std::vector<double>)>
                    write_batch,
                int batch_size, Options options) {
    BOOST_LOG_TRIVIAL(info) << "Starting Gibbs Sampling";

    int n_categories = 0;
    if (data.size() > 0) {
        n_categories = data.at(0).size();
    }

    std::default_random_engine generator;
    generator.seed(std::chrono::system_clock::now().time_since_epoch().count());

    std::uniform_real_distribution<double> parameter_distribution(0.0, 1.0);
    std::vector<double> epsilons = {parameter_distribution(generator)};
    std::vector<delta_t> deltas = {
        sample_symmetric_simplex(parameter_distribution, generator, n_categories)};
    // Each item contains a vector of the estimated alpha of each object at each timestep.
    std::vector<std::vector<alpha_t>> models;
    models.push_back(std::vector<alpha_t>());

    ModelDistribution model_distribution(data, generator, options);
    ModelSampler sampler(data, generator, options);

    std::vector<double> log_likelyhoods;

    for (int iteration = 1; iteration <= iterations; ++iteration) {
        BOOST_LOG_TRIVIAL(info) << "Iteration " << iteration;

        if (options.fixed_alphas.empty()) {
            std::vector<alpha_t> new_models =
                sampler.sample(epsilons.at(iteration - 1), deltas.at(iteration - 1));
            models.push_back(new_models);
        } else {
            models.push_back(options.fixed_alphas);
        }

        epsilons.push_back(MetropolisHastingsSampler::sample<double, std::default_random_engine>(
            10,  // Iterations
            [delta = deltas.at(iteration - 1), model = models.at(iteration),
             model_distribution](double epsilon) {
                std::vector<std::vector<alpha_t>> alphas_per_object(model.size());
                for (int i = 0; i < alphas_per_object.size(); ++i) {
                    alphas_per_object.at(i) = std::vector<alpha_t>(1, model.at(i));
                }
                std::vector<std::vector<double>> log_alpha_likelyhoods_per_object =
                    model_distribution.distribution(alphas_per_object, epsilon, delta);
                std::vector<double> log_alpha_likelyhoods(log_alpha_likelyhoods_per_object.size());
                std::transform(log_alpha_likelyhoods_per_object.begin(),
                               log_alpha_likelyhoods_per_object.end(),
                               log_alpha_likelyhoods.begin(),
                               [](std::vector<double> v) { return v[0]; });

                return std::accumulate(log_alpha_likelyhoods.begin(), log_alpha_likelyhoods.end(),
                                       0.0);
            },  // log_pdf
            [&parameter_distribution](std::default_random_engine &generator) {
                return parameter_distribution(generator);
            },  // uniform_sampler
            [](double center, std::default_random_engine &generator) {
                std::normal_distribution<> dist(center, 0.25);
                return sample_probability(dist, generator);
            },  // conditional_sampler
            generator));

        deltas.push_back(MetropolisHastingsSampler::sample<delta_t, std::default_random_engine>(
            10,  // Iterations
            [epsilon = epsilons.at(iteration), model = models.at(iteration),
             model_distribution](delta_t delta) {
                std::vector<std::vector<alpha_t>> alphas_per_object(model.size());
                for (int i = 0; i < alphas_per_object.size(); ++i) {
                    alphas_per_object.at(i) = std::vector<alpha_t>(1, model.at(i));
                }
                std::vector<std::vector<double>> log_alpha_likelyhoods_per_object =
                    model_distribution.distribution(alphas_per_object, epsilon, delta);
                std::vector<double> log_alpha_likelyhoods =
                    flatten<double>(log_alpha_likelyhoods_per_object);

                return accumulate(log_alpha_likelyhoods.begin(), log_alpha_likelyhoods.end(), 0.0);
            },  // log_pdf
            [parameter_distribution, n_categories](std::default_random_engine &generator) {
                return sample_symmetric_simplex(parameter_distribution, generator, n_categories);
            },  // uniform_sampler
            [](delta_t center, std::default_random_engine &generator) {
                return sample_gaussian_simplex<>(center, 0.25, generator);
            },  // Conditional sampler
            generator));

        if (options.record_likelyhood) {
            std::vector<double> log_likelyhood_per_object =
                flatten<double>(model_distribution.distribution(
                    models.at(iteration), epsilons.at(iteration), deltas.at(iteration)));
            log_likelyhoods.push_back(std::accumulate(log_likelyhood_per_object.begin(),
                                                      log_likelyhood_per_object.end(), 0.0));
        }

        if (iteration % batch_size == batch_size - 1 || iteration == iterations) {
            int batch = iteration / batch_size + 1;
            int start_index = (batch - 1) * batch_size;
            int end_index = std::min(batch * batch_size, iterations + 1);

            std::vector<std::vector<alpha_t>> alpha_batch(models.begin() + start_index,
                                                          models.begin() + end_index);
            std::vector<double> epsilon_batch(epsilons.begin() + start_index,
                                              epsilons.begin() + end_index);
            std::vector<delta_t> delta_batch(deltas.begin() + start_index,
                                             deltas.begin() + end_index);
            std::vector<double> log_likelyhood_batch;
            if (options.record_likelyhood) {
                log_likelyhood_batch = std::vector<double>(log_likelyhoods.begin() + start_index,
                                                           log_likelyhoods.begin() + end_index);
            }

            write_batch(alpha_batch, epsilon_batch, delta_batch, log_likelyhood_batch);
        }
    }

    return std::make_tuple(models, epsilons, deltas, log_likelyhoods);
}

/**
 * Reads in a csv file of category_count_t objects in order.
 * See category_count_t in types.hpp for details.
 * Expects a header line.
 */
std::vector<category_counts_t> read_category_counts_file(std::string in_path) {
    std::ifstream in_file(in_path);
    std::string line;
    std::vector<category_counts_t> data;
    if (in_file.is_open()) {
        // Trim the header line
        getline(in_file, line);
        while (getline(in_file, line)) {
            std::vector<std::string> items;
            boost::split(items, line, boost::is_any_of(","));
            category_counts_t datum = {std::stoi(items.at(1)), std::stoi(items.at(2)),
                                       std::stoi(items.at(3))};
            data.push_back(datum);
        }
        in_file.close();
    } else {
        BOOST_LOG_TRIVIAL(fatal) << "File " << in_path << " not found.";
        assert(false);
    }

    return data;
}

std::vector<alpha_t> read_alphas(std::string alpha_path) {
    std::ifstream in_file(alpha_path);
    std::string line;
    std::vector<alpha_t> alphas;
    if (in_file.is_open()) {
        // Trim the header line
        getline(in_file, line);
        while (getline(in_file, line)) {
            std::vector<std::string> items;
            boost::split(items, line, boost::is_any_of(","));
            alpha_t alpha = {bool(std::stoi(items.at(1))), bool(std::stoi(items.at(2))),
                             bool(std::stoi(items.at(3)))};
            alphas.push_back(alpha);
        }
        in_file.close();
    } else {
        BOOST_LOG_TRIVIAL(fatal) << "File " << alpha_path << " not found.";
        assert(false);
    }

    return alphas;
}

std::ofstream setup_output(std::string out_path) {
    return std::ofstream(out_path, std::ios::out | std::ios::trunc);
}

/**
 * Sets the current logging level.
 */
void initLogging() {
    logging::core::get()->set_filter(logging::trivial::severity >= logging::trivial::info);
}

int main(int argc, char *const *argv) {
    initLogging();

    // Setting up command line flags.
    CLI::App app{"Input Filter Model"};

    std::string in_path = "input.csv";
    app.add_option("-i,--in", in_path,
                   "The path to a csv file containing pairs of direct object and object counts.")
        ->required();

    std::string out_path = "output.csv";
    app.add_option("-o,--output", out_path, "The path of the output file.")->required();

    int iterations = 1;
    app.add_option("--iterations", iterations,
                   "The number of iterations to run Gibbs sampling for.");

    std::string alpha_path;
    CLI::Option *alpha_path_option = app.add_option(
        "--alpha-path", alpha_path, "Path to fixed alpha values to use, only if provided.");

    std::string message;
    CLI::Option *message_option =
        app.add_option("-m, --message", message, "Message to be written at the top of the output.")
            ->required();

    bool record_likelyhood = false;
    CLI::Option *record_likelyhood_flag =
        app.add_flag("--record-likelyhood", record_likelyhood,
                     "Additionally record the likelyhood of the sampled parameters.");

    bool exact = false;
    CLI::Option *exact_flag =
        app.add_flag("--exact", exact, "Prevent approximations in calcualtion.");

    bool comparison = false;
    app.add_flag("--comparison", comparison,
                 "If both integration methods shosuld be used and compared.")
        ->excludes(exact_flag);

    bool use_smaller_alphas = false;
    CLI::Option *use_smaller_alphas_flag = app.add_flag("--use-smaller-alphas", use_smaller_alphas,
                                                        "Only consider alphas that are possible.")
                                               ->excludes(alpha_path_option);

    CLI11_PARSE(app, argc, argv);

    std::vector<alpha_t> alphas;
    if (!alpha_path.empty()) {
        alphas = read_alphas(alpha_path);
    }

    Options options;
    options.comparison = comparison;
    options.exact = exact;
    options.use_smaller_alphas = use_smaller_alphas;
    options.record_likelyhood = record_likelyhood;
    options.fixed_alphas = alphas;

    auto data = read_category_counts_file(in_path);

    std::ofstream out_file = setup_output(out_path);

    out_file << "Comment: " << message << ", Input path: " << in_path
             << ", Output path: " << out_path << ", Alpha path: " << alpha_path
             << ", Iterations: " << std::to_string(iterations)
             << ", Comparison: " << std::to_string(options.comparison)
             << ", Exact: " << std::to_string(options.exact)
             << ", Use smaller alphas: " << std::to_string(options.use_smaller_alphas)
             << ", Record likelyhood: " << std::to_string(options.record_likelyhood) << std::endl;

    auto write_batch = [&out_file](std::vector<std::vector<alpha_t>> alpha_batch,
                                   std::vector<double> epsilon_batch,
                                   std::vector<delta_t> delta_batch,
                                   std::vector<double> log_likelyhood_batch) {
        for (int i = 0; i < alpha_batch.size(); ++i) {
            if (!log_likelyhood_batch.empty()) {
                out_file << vector_of_vector_to_string<>(alpha_batch.at(i)) << ","
                         << epsilon_batch.at(i) << "," << vector_to_string<>(delta_batch.at(i))
                         << "," << log_likelyhood_batch.at(i) << std::endl;
            } else {
                out_file << vector_of_vector_to_string<>(alpha_batch.at(i)) << ","
                         << epsilon_batch.at(i) << "," << vector_to_string<>(delta_batch.at(i))
                         << std::endl;
            }
        }
    };

    joint_inference(data, iterations, write_batch, 100, options);

    BOOST_LOG_TRIVIAL(info) << "Inference complete.";

    out_file.close();
}