#include "imgproc.h"
#include "debug.h"
#include "v128.h"

#if defined(WITH_TBB)
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#endif

#include <stdexcept>
#include <cmath>

#if defined(WITH_OPENCV)
#include <opencv2/imgproc/imgproc.hpp>
#endif

#include <smmintrin.h>

#include <Eigen/Dense>

#include <opencv2/core/core.hpp>

/**
 * comptue the image gradient for a row of pixels in the image
 */
template <typename TSrc, typename TDst> FORCE_INLINE
void imgradient_row(const TSrc* srow, int cols, TDst* Ix_row, TDst* Iy_row)
{
  static_assert( std::is_signed<TDst>::value, "imgradient destination type must be signed" );

  constexpr auto S = imgradient_scale<TDst>();

  Ix_row[0] = TDst(0);
  Iy_row[0] = TDst(0);

  for(int x = 1; x < cols - 1; ++x) {
    Ix_row[x] = S * (TDst(srow[x+1]) - TDst(srow[x-1]));
    Iy_row[x] = S * (TDst(srow[x+cols]) - TDst(srow[x-cols]));
  }

  Ix_row[cols-1] = TDst(0);
  Iy_row[cols-1] = TDst(0);
}

#if defined(WITH_TBB)
template <typename TSrc>
struct ImageGradientFunc_
{
  ImageGradientFunc_(const TSrc* src_ptr, int cols, float* Ix_ptr, float* Iy_ptr)
      : _src(src_ptr), _cols(cols), _Ix(Ix_ptr), _Iy(Iy_ptr) {}

  void operator()(const tbb::blocked_range<int>& range) const
  {
    for(int y = range.begin(); y != range.end(); ++y)
    {
      auto srow = _src + y*_cols;
      auto Ix_row = _Ix + y*_cols;
      auto Iy_row = _Iy + y*_cols;

      imgradient_row(srow, _cols, Ix_row, Iy_row);
    }
  }

 private:
  const TSrc* _src;
  int _cols;
  float* _Ix;
  float* _Iy;
}; // ImageGradientFunc
#endif

template <typename TSrc = uint8_t> inline
void imgradient_(const TSrc* src_ptr, const ImageSize& im_size, float* Ix_ptr, float* Iy_ptr)
{
  auto rows = im_size.rows;
  auto cols = im_size.cols;

  memset(Ix_ptr, 0.0, sizeof(float) * cols);
  memset(Iy_ptr, 0.0, sizeof(float) * cols);

#if defined(WITH_TBB)
  tbb::parallel_for(tbb::blocked_range<int>(1, rows-1),
                    ImageGradientFunc_<TSrc>(src_ptr, cols, Ix_ptr, Iy_ptr));
#else
  for(int y = 1; y < rows - 1; ++y) {
    auto srow = src_ptr + y*cols;
    auto Ix_row = Ix_ptr + y*cols;
    auto Iy_row = Iy_ptr + y*cols;
    imgradient_row(srow, cols, Ix_row, Iy_row);
  }
#endif
  memset(Ix_ptr + (rows-1)*cols, 0.0f, sizeof(float) * cols);
  memset(Iy_ptr + (rows-1)*cols, 0.0f, sizeof(float) * cols);
}

void imgradient(const uint8_t* src_ptr, const ImageSize& im_size, float* Ix_ptr, float* Iy_ptr)
{
  imgradient_(src_ptr, im_size, Ix_ptr, Iy_ptr);
}

void imgradient(const float* src_ptr, const ImageSize& im_size, float* Ix_ptr, float* Iy_ptr)
{
  imgradient_(src_ptr, im_size, Ix_ptr, Iy_ptr);
}

using bpvo::v128;

static const __m128i K0x01 = _mm_set1_epi8(0x01);
static const __m128i K0x02 = _mm_set1_epi8(0x02);
static const __m128i K0x04 = _mm_set1_epi8(0x04);
static const __m128i K0x08 = _mm_set1_epi8(0x08);
static const __m128i K0x10 = _mm_set1_epi8(0x10);
static const __m128i K0x20 = _mm_set1_epi8(0x20);
static const __m128i K0x40 = _mm_set1_epi8(0x40);
static const __m128i K0x80 = _mm_set1_epi8(0x80);

/**
 * computes the Census Transform for 16 pixels at once
 */
static FORCE_INLINE void censusOp(const uint8_t* src, int stride, uint8_t* dst)
{

#define C_OP >=
  const v128 c(src);
  _mm_storeu_si128((__m128i*) dst,
                   ((v128(src - stride - 1) C_OP c) & K0x01) |
                   ((v128(src - stride    ) C_OP c) & K0x02) |
                   ((v128(src - stride + 1) C_OP c) & K0x04) |
                   ((v128(src          - 1) C_OP c) & K0x08) |
                   ((v128(src          + 1) C_OP c) & K0x10) |
                   ((v128(src + stride - 1) C_OP c) & K0x20) |
                   ((v128(src + stride    ) C_OP c) & K0x40) |
                   ((v128(src + stride + 1) C_OP c) & K0x80));
#undef C_OP
}

static FORCE_INLINE void census_row(const uint8_t* srow, int cols, uint8_t* drow)
{
  drow[0] = 0;

  const int W = 1 + ((cols - 2) & ~15);
  for(int c = 1; c < W; c += 16) {
    censusOp(srow + c, cols, drow + c);
  }

  if(W != (cols - 1)) {
    censusOp(srow + cols - 1 - 16, cols, drow + cols - 1 - 16);
  }

  drow[cols-1] = 0;
}

#if defined(WITH_TBB)
struct CensusTransformFunc
{
  CensusTransformFunc(const uint8_t* src_ptr, int cols, uint8_t* dst_ptr)
      : _src(src_ptr), _cols(cols), _dst(dst_ptr) {}

  void operator()(const tbb::blocked_range<int>& range) const
  {
    for(int y = range.begin(); y != range.end(); ++y)
    {
      auto srow = _src + y*_cols;
      auto drow = _dst + y*_cols;
      census_row(srow, _cols, drow);
    }
  }

 private:
  const uint8_t* _src;
  int _cols;
  uint8_t* _dst;
}; // CensusOpRow
#endif

Image_<uint8_t> censusTransform(const uint8_t* I_ptr, const ImageSize& im_size)
{
  Image_<uint8_t> ret(im_size.rows, im_size.cols);
  auto dst_ptr = ret.data();

  memset(dst_ptr, 0, im_size.cols);

#if defined(WITH_TBB)
  tbb::parallel_for(tbb::blocked_range<int>(1, im_size.rows-1),
                    CensusTransformFunc(I_ptr, im_size.cols, dst_ptr));
#else
  for(int y = 1; y < im_size.rows - 1; ++y) {
    auto srow = I_ptr + y*im_size.cols;
    auto drow = dst_ptr + y*im_size.cols;
    census_row(srow, im_size.cols, drow);
  }
#endif

  memset(dst_ptr + (im_size.rows-1)*im_size.cols, 0, im_size.cols);
  return ret;
}

static inline
void ExtractBitPlanesChannel(const uint8_t* src, const ImageSize& image_size,
                             int bit, float* dst, float sigma)
{
  int N = image_size.rows * image_size.cols;
  for(int i = 0; i < N; ++i) {
    dst[i] = static_cast<float>( (src[i] & (1 << bit)) >> bit );
  }

  if(sigma > 0.0) {
    cv::Mat dst_(image_size.rows, image_size.cols, cv::DataType<float>::type, (void*) dst);
    cv::GaussianBlur(dst_, dst_, cv::Size(5,5), sigma, sigma);
  }
}

void computeBitPlanes(const uint8_t* image, const ImageSize& im_size,
                      EigenAlignedContainer_<Image_<float>>& dst, float sigma_ct, float sigma_bp)
{
  auto rows = im_size.rows, cols = im_size.cols;
  const uint8_t* src_ptr = nullptr;
  cv::Mat I_s;
  if(sigma_ct > 0.0f) {
    const cv::Mat I(rows, cols, cv::DataType<uint8_t>::type, (void*) image);
    cv::GaussianBlur(I, I_s, cv::Size(3, 3), sigma_ct);
    src_ptr = I_s.ptr<const uint8_t>();
  } else {
    src_ptr = image;
  }

  const auto C = censusTransform(src_ptr, im_size);

  if(dst.size() != 8)
    dst.resize(8);

#if defined(WITH_OPENMP)
#pragma omp parallel for
#endif
  for(size_t i = 0; i < 8; ++i) {
    if(dst[i].rows() != rows || dst[i].cols() != cols)
      dst[i].resize(rows, cols);
    ExtractBitPlanesChannel(C.data(), ImageSize(rows, cols), i, dst[i].data(), sigma_bp);
  }
}

void imsmooth(const float* src, const ImageSize& im_size, int ks, double s, float* dst)
{
#if defined(WITH_OPENCV)
  const cv::Mat src_(im_size.rows, im_size.cols, cv::DataType<float>::type, (void*) src);
  cv::Mat dst_(im_size.rows, im_size.cols, cv::DataType<float>::type, (void*) dst);

  cv::GaussianBlur(src_, dst_, cv::Size(ks, ks), s, s);
#else
#error "compile WITH_OPENCV"
#endif

}

void imsmooth(const uint8_t* src, const ImageSize& im_size, int ks, double s, float* dst)
{
#if defined(WITH_OPENCV)
  const cv::Mat src_(im_size.rows, im_size.cols, cv::DataType<uint8_t>::type, (void*) src);
  cv::Mat src_f;
  src_.convertTo(src_f, CV_32F);

  imsmooth(src_f.ptr<const float>(), im_size, ks, s, dst);

#else
#error "compile WITH_OPENCV"
#endif
}

template <typename T, size_t A = 16>
static bool inline is_aligned(const T* p)
{
  return 0 == (std::ptrdiff_t(p) & (A-1));
}

void disparityToDepth(const float* dmap, const ImageSize& im_size, float Bf, float* zmap)
{
  constexpr float MinValidDisparity = 0.01f;
  constexpr float InvalidDepthMark  = -.10f;

  const auto MD = _mm_set1_ps(MinValidDisparity);
  const auto IV = _mm_set1_ps(InvalidDepthMark);
  const auto BF = _mm_set1_ps(Bf);

  const auto N = im_size.numel();
  int i = 0;

  if(is_aligned(dmap) && is_aligned(zmap)) {
#if defined(WITH_OPENMP)
#pragma omp parallel for
#endif
    for(i = 0; i <= N - 4; i += 4) {
      auto d = _mm_load_ps(dmap + i);
      auto m = _mm_cmpgt_ps(d, MD);
      auto z = _mm_mul_ps(BF, _mm_rcp_ps(d));
      auto zz = _mm_blendv_ps(IV, z, m);
      _mm_store_ps(zmap + i, zz);
    }
  } else {
#if defined(WITH_OPENCV)
#pragma omp parallel for
#endif
    for(i = 0; i <= N - 4; i += 4) {
      auto d = _mm_loadu_ps(dmap + i);
      auto m = _mm_cmpgt_ps(d, MD);
      auto z = _mm_mul_ps(BF, _mm_rcp_ps(d));
      auto zz = _mm_blendv_ps(IV, z, m);
      _mm_storeu_ps(zmap + i, zz);
    }
  }

  if(N % 16) {
    for(i = N-4; i < N; ++i) {
      zmap[i] = dmap[i] > MinValidDisparity ? (Bf  * (1.0f / dmap[i])) : -1.0f;
    }
  }
}

void disparityToDepth(const cv::Mat& d, double bf, cv::Mat_<float>& z)
{
  assert( d.type() == cv::DataType<float>::type );
  z.create(d.size());

  disparityToDepth(d.ptr<const float>(), ImageSize(d.rows, d.cols), bf, z.ptr<float>());
}


/**
 *          Copyright Matthias Walter 2010.
 * 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)
 **/

#ifndef MATROID_GRAPH_HPP_
#define MATROID_GRAPH_HPP_

#include <iostream>

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

namespace unimod
{

  /**
   * Matroid element property
   */

  enum edge_matroid_element_t
  {
    edge_matroid_element
  };

  typedef boost::property <unimod::edge_matroid_element_t, int> matroid_element_property;
  typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, matroid_element_property> matroid_graph;

}

namespace boost
{

  /**
   * Matroid element property tag
   */

  template <>
  struct property_kind <unimod::edge_matroid_element_t>
  {
    typedef edge_property_tag type;
  };

}

namespace unimod
{

  typedef boost::property_map <matroid_graph, edge_matroid_element_t>::const_type const_matroid_element_map;
  typedef boost::property_map <matroid_graph, edge_matroid_element_t>::type matroid_element_map;

  /**
   * Output operator for a matroid graph
   *
   * @param stream Output stream
   * @param graph The given matroid graph
   * @return The output stream after writing to it
   */

  std::ostream& operator<<(std::ostream& stream, const unimod::matroid_graph& graph);

}

#endif /* MATROID_GRAPH_HPP_ */


// General ROS functionality
#include <ros/ros.h>

// PI
#include "pi/PIController.h"
#include "pi/global.h"
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>
#include <boost/thread/mutex.hpp>
#include "std_msgs/String.h"
#include <algorithm>
// END PI

// Quadrotor services
#include <hal_quadrotor/State.h>

// Quadrotor controllers
#include <hal_quadrotor/Hover.h>
#include <hal_quadrotor/control/Takeoff.h>
#include <hal_quadrotor/control/VelocityHeight.h>

#include <hal_quadrotor/GetEstimate.h>
#include <hal_quadrotor/SetTruth.h>

#define EPS_POS			5e-1	// epsilon position
#define EPS_VEL			1e-3	// epsilon position

//#define DEBUG_INFO 0

typedef enum { IDLE, TAKEOFF, VELHEIGHT, HOVER, WAYPOINT, OTHER } FSM;

using namespace std;

vector<ros::ServiceClient> srvTakeOff;
vector<ros::ServiceClient> srvHover;
vector<ros::ServiceClient> srvVelocityHeight;

vector<FSM> state_FSM; // Finite State Machine state (TODO: get the controller from the msg)
vector<double> jstate;

// used to safely exit the program
bool stop;

// PI
PIController pi;
// END PI

vector<double> heights;
vector<string> idunits;
int units;

boost::mutex guard;

// Rotation from body to navigation frame
void b2n(double rot[3], double vec[3])
{
    double tmp[3];
    for (int i = 0; i < 3; i++)
        tmp[i] = vec[i];
    double sph = sin(rot[0]); double cph = cos(rot[0]);
    double sth = sin(rot[1]); double cth = cos(rot[1]);
    double sps = sin(rot[2]); double cps = cos(rot[2]);
    double dcm[3][3];
    dcm[0][0] = cth*cps;
    dcm[1][0] = cth*sps;
    dcm[2][0] = -sth;
    dcm[0][1] = sph*sth*cps - cph*sps;
    dcm[1][1] = sph*sth*sps + cph*cps;
    dcm[2][1] = sph*cth;
    dcm[0][2] = cph*sth*cps + sph*sps;
    dcm[1][2] = cph*sth*sps - sph*cps;
    dcm[2][2] = cph*cth;
    for (int i = 0; i < 3; i++)
    {
        vec[i] = 0;
        for (int j = 0; j < 3; j++)
            vec[i] += dcm[i][j] * tmp[j];
    }
}

// Callback for quadrotor state
void StateCallback(int unit, const hal_quadrotor::State::ConstPtr& msg)
{
	if (msg->controller == (std::string) "Hover" || msg->controller == (std::string) "VelocityHeight")
	{
		state_FSM[unit]  = HOVER;
				// angles of the platform
		double rot[3] = {msg->roll, msg->pitch, msg->yaw};

		// put BODY velocity in vel, and rotate to NAVIGATIONAL velocity
		double vel[3] = {msg->u, msg->v, msg->w};
		b2n(rot, vel);

		// update internal state
		jstate[unit*4+0] = msg->x;
		jstate[unit*4+1] = msg->y;
		jstate[unit*4+2] = vel[0];
		jstate[unit*4+3] = vel[1];
#ifdef DEBUG_INFO
		ROS_INFO("UAV %d got state (x, y, dx, dy) = (%f, %f, %f, %f)", unit, msg->x, msg->y, vel[0], vel[1]);
#endif
	}
}

void stopCallback(const std_msgs::String::ConstPtr& msg)
{
#ifdef DEBUG_INFO
	ROS_INFO("Received: PI_STOP.\nSwitching all units to Hover.");
#endif
	stop = true;
}

// Main entry point of application
int main(int argc, char **argv)
{
	if (argc!=2) {
	  ROS_FATAL("requires xml_file as argument");
	  return 1;
	}

	try {
		ifstream is(argv[1]);
		// read parameters from xml file
		using boost::property_tree::ptree;
		ptree pt;
		if (!is.is_open()) {
		  ROS_FATAL("xml config not found");
		  return 1;
		}
		read_xml(is, pt);

		// get vector of desired heights
		BOOST_FOREACH(ptree::value_type &v,	pt.get_child("takeoffz"))
			heights.push_back(v.second.get<double>(""));

		// get vector of ids
		BOOST_FOREACH(ptree::value_type &v,	pt.get_child("idunits"))
			idunits.push_back(v.second.get<string>(""));

		// initialize Joint State variable
		units = pt.get<int>("units");
		jstate.resize( units*pt.get<int>("dimUAVx") );

		// Initialize PI controller
		pi.Init(pt);

		//double T = pt.get<double>("T");

	} catch (int e) {
		cout << "Exception" << e << endl;
	}

	// used to safely exit the program
	stop = false;

	// Initialise the ROS client
	string nodestr = string("main_suav");
	ros::init(argc, argv, nodestr.c_str());

	// Create a node handle, which this C code will use to bind to the ROS server
	ros::NodeHandle n;

	// Try and get a list of topics currently being braodcast
	ros::master::V_TopicInfo topics;
	int units_found = 0;
	vector<string> names;
	vector<ros::Subscriber> top_state;

	// Iterate over the topics, checking for entities on /hal/<type>/<model>/<id>/Estimate
	if (ros::master::getTopics(topics)) {
		for (ros::master::V_TopicInfo::iterator i = topics.begin(); i != topics.end(); i++) {
			boost::cmatch mr;
			if (boost::regex_match(i->name.c_str(), mr, boost::regex("^/hal/quadrotor/(.+)/(.+)/Estimate$"))) {
				ROS_INFO("%s -> %s", i->name.c_str(), i->datatype.c_str());
				names.push_back(i->name);
				units_found++;
			}
		}
	}

	// check if number of units found corresponds to the expected units (defined in the xml)
	if ( units_found != units ) {
	  ROS_FATAL("Units found %d does not match units defined in the config %d: ", units_found, units);
	  return 1;
	}

	// sort the names (currently required, since the joint state is indexed by unit)
	sort(names.begin(), names.end());

	// Subscribe to a stop message
	ros::Subscriber sub_stop = n.subscribe("PI_STOP", 1000, stopCallback);

	// subscribe and add controllers for each unit
	for (int i=0; i<units; i++) {

		// init Finite State Machine
		state_FSM.push_back(IDLE);

		ROS_INFO("%s",names[i].c_str());

		// Subscribe to the states of the quadrotors (requires explicit)
		top_state.push_back( n.subscribe<hal_quadrotor::State>(
			names[i].c_str(),10,boost::bind(StateCallback, i, _1))
		);
		ROS_INFO("%s subscribed!", names[i].c_str());

		// Add TakeOff controller
		string takeoff_cmd = string("/hal/quadrotor/hummingbird/") + idunits[i] + string("/controller/Takeoff");
		srvTakeOff.push_back(n.serviceClient<hal_quadrotor::Takeoff>(takeoff_cmd.c_str()) );

		// Add Hover controller
		string hover_cmd = string("/hal/quadrotor/hummingbird/") + idunits[i] + string("/controller/Hover");
		srvHover.push_back(n.serviceClient<hal_quadrotor::Hover>(hover_cmd.c_str()) );

		// Add VelocityHeight controller
		string velheight_cmd = string("/hal/quadrotor/hummingbird/") + idunits[i] + string("/controller/VelocityHeight");
		srvVelocityHeight.push_back( n.serviceClient<hal_quadrotor::VelocityHeight>(velheight_cmd.c_str()) );
	}

	// Call Takeoff on each unit
	for (int i=0; i<units; i++) {
		hal_quadrotor::Takeoff msgTakeoff;
		msgTakeoff.request.altitude = heights[i];
		if(!srvTakeOff[i].call(msgTakeoff)){
			ROS_FATAL("NO TAKE OFF!!");
		} else {
			ROS_INFO("Taking OFF!!");
		}
		state_FSM[i] = TAKEOFF;
	}

	// Keep going indefinitely
	ros::Rate r(15);
    while (ros::ok())
    {
    	// Collect all state estimates
		ros::spinOnce();

		// check stop condittion
		if (stop)
		{
			bool success = true;
			// Call Hover on each unit
			for (int i=0; i<units; i++)
			{
				hal_quadrotor::Hover msgHover;
				if(!srvHover[i].call(msgHover))
				{
					success = false;
					ROS_FATAL("NO Hover for UAV %d!!", i);
				}
			}
			if(success)
			{
				ROS_INFO("Succesfully switched all units to Hover.");
				break;
				return 0;
			}
		}

		// Check that units are ready to receive VelocityHeight commands
		bool ready = true;
		for (int i=0; i<units; i++) {
			ready = ready && (state_FSM[i] == HOVER);
		}
		if (ready)
		{
			// Path Integral controller (based on VelocityHeight)
			vec X = jstate;
			//vec action = pi.computeControl(X);
			vec action = pi.computeControlFeedback(X);
			vec Xp = pi.predictState(X, action);

			// Distribute commands
			for (int i=0; i<units; i++) {

				// convert pi control to VelocityHeight control
				hal_quadrotor::VelocityHeight msgVelocityHeight;
				msgVelocityHeight.request.dx  = Xp[i*4 + 2];
				msgVelocityHeight.request.dy  = Xp[i*4 + 3];
				msgVelocityHeight.request.z   = heights[i];
				msgVelocityHeight.request.yaw = 0.;

#ifdef DEBUG_INFO
				ROS_INFO("UAV %d sent command (dx, dy, z, yaw) = (%f, %f, %f, %f)",
					i, msgVelocityHeight.request.dx, msgVelocityHeight.request.dy,
					msgVelocityHeight.request.z, msgVelocityHeight.request.yaw);
#endif

				if(!srvVelocityHeight[i].call(msgVelocityHeight)){
					ROS_FATAL("NO VELHEIGHT!!");
				}
			}
		}

		// Sleep
		r.sleep();
    }

	ROS_INFO("Terminating PI control (muav_main).");
	ros::spin();

	// Success!
	return 0;
}


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file bes_test.cpp
/// \brief Some tests for BES.

#define BOOST_TEST_MODULE bes_test
#include "mcrl2/bes/boolean_equation_system.h"
#include "mcrl2/bes/find.h"
#include "mcrl2/bes/join.h"
#include "mcrl2/bes/print.h"
#include <boost/test/included/unit_test_framework.hpp>

using namespace mcrl2;
using namespace mcrl2::bes;

typedef core::term_traits<boolean_expression> tr;

void test_join()
{
  boolean_variable X("X");
  const boolean_expression& Z1 = X;
  const boolean_expression& Z2(X);
  boolean_expression Z3;
  Z3 = X;
  Z3=Z2;

  std::set<boolean_expression> s;
  s.insert(boolean_variable("X1"));
  s.insert(boolean_variable("X2"));
  boolean_expression x = join_or(s.begin(), s.end());
  std::cout << "x = " << bes::pp(x) << std::endl;

#ifdef MCRL2_JOIN_TEST
// The gcc compiler gives the following error:
//
// D:\mcrl2\libraries\core\include/mcrl2/utilities/detail/join.h:54:22: error: call
// of overloaded 'boolean_expression(const mcrl2::bes::boolean_variable&)' is
// ambiguous
//
// This seems to be triggered by an incorrect optimization, in which the return
// type of the function false_() is discarded.
//
  std::set<boolean_variable> sv;
  sv.insert(boolean_variable("X1"));
  sv.insert(boolean_variable("X2"));
  x = join_or(sv.begin(), sv.end());
  std::cout << "x = " << bes::pp(x) << std::endl;
#endif

}

void test_expressions()
{
  boolean_variable X("X");
  boolean_variable Y("Y");

  BOOST_CHECK(tr::is_prop_var(X));
  BOOST_CHECK(tr::is_prop_var(Y));

  boolean_expression true_(tr::true_());
  boolean_expression false_(tr::false_());
  boolean_expression not_(tr::not_(X));
  boolean_expression and_(tr::and_(X,Y));
  boolean_expression or_(tr::or_(X,Y));
  boolean_expression imp(tr::imp(X,Y));

  BOOST_CHECK(tr::is_true(true_));
  BOOST_CHECK(!tr::is_true(false_));
  BOOST_CHECK(!tr::is_true(not_));
  BOOST_CHECK(!tr::is_true(and_));
  BOOST_CHECK(!tr::is_true(or_));
  BOOST_CHECK(!tr::is_true(imp));

  BOOST_CHECK(!tr::is_false(true_));
  BOOST_CHECK(tr::is_false(false_));
  BOOST_CHECK(!tr::is_false(not_));
  BOOST_CHECK(!tr::is_false(and_));
  BOOST_CHECK(!tr::is_false(or_));
  BOOST_CHECK(!tr::is_false(imp));

  BOOST_CHECK(!tr::is_not(true_));
  BOOST_CHECK(!tr::is_not(false_));
  BOOST_CHECK(tr::is_not(not_));
  BOOST_CHECK(!tr::is_not(and_));
  BOOST_CHECK(!tr::is_not(or_));
  BOOST_CHECK(!tr::is_not(imp));

  BOOST_CHECK(!tr::is_and(true_));
  BOOST_CHECK(!tr::is_and(false_));
  BOOST_CHECK(!tr::is_and(not_));
  BOOST_CHECK(tr::is_and(and_));
  BOOST_CHECK(!tr::is_and(or_));
  BOOST_CHECK(!tr::is_and(imp));

  BOOST_CHECK(!tr::is_or(true_));
  BOOST_CHECK(!tr::is_or(false_));
  BOOST_CHECK(!tr::is_or(not_));
  BOOST_CHECK(!tr::is_or(and_));
  BOOST_CHECK(tr::is_or(or_));
  BOOST_CHECK(!tr::is_or(imp));

  BOOST_CHECK(!tr::is_imp(true_));
  BOOST_CHECK(!tr::is_imp(false_));
  BOOST_CHECK(!tr::is_imp(not_));
  BOOST_CHECK(!tr::is_imp(and_));
  BOOST_CHECK(!tr::is_imp(or_));
  BOOST_CHECK(tr::is_imp(imp));
}

void test_boolean_equation()
{
  boolean_variable X("X");
  boolean_variable Y("Y");
  boolean_variable Z("Z");

  boolean_equation e(fixpoint_symbol::nu(), X, tr::and_(Y,Z));
  BOOST_CHECK(e.symbol() == fixpoint_symbol::nu());
  BOOST_CHECK(e.variable() == X);
  BOOST_CHECK(e.formula() == tr::and_(Y,Z));

  // Check for finding variables in the right hand side
  std::set<boolean_variable> expected;
  expected.insert(Y);
  expected.insert(Z);

  std::set<boolean_variable> found;

  find_boolean_variables(Y, std::inserter(found, found.end()));
  find_boolean_variables(Z, std::inserter(found, found.end()));
  BOOST_CHECK(found == expected);

  find_boolean_variables(tr::and_(Y,Z), std::inserter(found, found.end()));
  BOOST_CHECK(found == expected);

  found.clear();
  find_boolean_variables(e.formula(), std::inserter(found, found.end()));
  BOOST_CHECK(found == expected);
}

void test_bes()
{
  boolean_variable X("X");
  boolean_variable Y("Y");
  boolean_variable Z("Z");

  boolean_equation eqX(fixpoint_symbol::nu(), X, tr::and_(X, tr::and_(Y,Z)));
  boolean_equation eqY(fixpoint_symbol::nu(), Y, tr::and_(X, Y));
  boolean_equation eqZ(fixpoint_symbol::mu(), Z, tr::or_(Z, X));
  std::vector<boolean_equation> eqns;
  eqns.push_back(eqX);
  eqns.push_back(eqY);
  eqns.push_back(eqZ);

  boolean_equation_system bes(eqns, X);
  BOOST_CHECK(bes.is_closed());

  bes::pp(bes);

  std::set<boolean_variable> occurring_variables = bes.occurring_variables();
  BOOST_CHECK(occurring_variables.size() == 3);
}

BOOST_AUTO_TEST_CASE(test_main)
{
  test_expressions();
  test_boolean_equation();
  test_join();
  test_bes();
}


#pragma once

#include "cho_util/core/geometry/geometry_base.hpp"

#include <Eigen/Core>

namespace cho {
namespace core {

template <typename _Scalar, int N>
class Point : public GeometryBase<Point<_Scalar, N>> {
  using Scalar = _Scalar;

 public:
  explicit constexpr Point() {}
  Point(std::initializer_list<Scalar> args) : data_(args) {}

  const Scalar* GetPtr() const { return data_.data(); }
  Scalar* GetPtr() { return data_.data(); }
  static constexpr int GetSize() { return N; }

  auto& GetData() { return data_; }
  const auto& GetData() const { return data_; }

 private:
  Eigen::Matrix<Scalar, N, 1> data_;

 public:
  friend class GeometryBase<Point>;
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <typename _Scalar, int N>
struct GeometryTraits<Point<_Scalar, N>> {
  using Scalar = _Scalar;
};
}  // namespace core
}  // namespace cho


/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    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 SFC_GEN_HPP
#define SFC_GEN_HPP

#include "geo_utils.hpp"
#include "firi.hpp"

#include <ompl/util/Console.h>
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/DiscreteMotionValidator.h>

#include <deque>
#include <memory>
#include <Eigen/Eigen>

namespace sfc_gen
{

    template <typename Map>
    inline double planPath(const Eigen::Vector3d &s,
                           const Eigen::Vector3d &g,
                           const Eigen::Vector3d &lb,
                           const Eigen::Vector3d &hb,
                           const Map *mapPtr,
                           const double &timeout,
                           std::vector<Eigen::Vector3d> &p)
    {
        auto space(std::make_shared<ompl::base::RealVectorStateSpace>(3));

        ompl::base::RealVectorBounds bounds(3);
        bounds.setLow(0, 0.0);
        bounds.setHigh(0, hb(0) - lb(0));
        bounds.setLow(1, 0.0);
        bounds.setHigh(1, hb(1) - lb(1));
        bounds.setLow(2, 0.0);
        bounds.setHigh(2, hb(2) - lb(2));
        space->setBounds(bounds);

        auto si(std::make_shared<ompl::base::SpaceInformation>(space));

        si->setStateValidityChecker(
            [&](const ompl::base::State *state)
            {
                const auto *pos = state->as<ompl::base::RealVectorStateSpace::StateType>();
                const Eigen::Vector3d position(lb(0) + (*pos)[0],
                                               lb(1) + (*pos)[1],
                                               lb(2) + (*pos)[2]);
                return mapPtr->query(position) == 0;
            });
        si->setup();

        ompl::msg::setLogLevel(ompl::msg::LOG_NONE);

        ompl::base::ScopedState<> start(space), goal(space);
        start[0] = s(0) - lb(0);
        start[1] = s(1) - lb(1);
        start[2] = s(2) - lb(2);
        goal[0] = g(0) - lb(0);
        goal[1] = g(1) - lb(1);
        goal[2] = g(2) - lb(2);

        auto pdef(std::make_shared<ompl::base::ProblemDefinition>(si));
        pdef->setStartAndGoalStates(start, goal);
        pdef->setOptimizationObjective(std::make_shared<ompl::base::PathLengthOptimizationObjective>(si));
        auto planner(std::make_shared<ompl::geometric::InformedRRTstar>(si));
        planner->setProblemDefinition(pdef);
        planner->setup();

        ompl::base::PlannerStatus solved;
        solved = planner->ompl::base::Planner::solve(timeout);

        double cost = INFINITY;
        if (solved)
        {
            p.clear();
            const ompl::geometric::PathGeometric path_ =
                ompl::geometric::PathGeometric(
                    dynamic_cast<const ompl::geometric::PathGeometric &>(*pdef->getSolutionPath()));
            for (size_t i = 0; i < path_.getStateCount(); i++)
            {
                const auto state = path_.getState(i)->as<ompl::base::RealVectorStateSpace::StateType>()->values;
                p.emplace_back(lb(0) + state[0], lb(1) + state[1], lb(2) + state[2]);
            }
            cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()).value();
        }

        return cost;
    }

    inline void convexCover(const std::vector<Eigen::Vector3d> &path,
                            const std::vector<Eigen::Vector3d> &points,
                            const Eigen::Vector3d &lowCorner,
                            const Eigen::Vector3d &highCorner,
                            const double &progress,
                            const double &range,
                            std::vector<Eigen::MatrixX4d> &hpolys,
                            const double eps = 1.0e-6)
    {
        hpolys.clear();
        const int n = path.size();
        Eigen::Matrix<double, 6, 4> bd = Eigen::Matrix<double, 6, 4>::Zero();
        bd(0, 0) = 1.0;
        bd(1, 0) = -1.0;
        bd(2, 1) = 1.0;
        bd(3, 1) = -1.0;
        bd(4, 2) = 1.0;
        bd(5, 2) = -1.0;

        Eigen::MatrixX4d hp, gap;
        Eigen::Vector3d a, b = path[0];
        std::vector<Eigen::Vector3d> valid_pc;
        std::vector<Eigen::Vector3d> bs;
        valid_pc.reserve(points.size());
        for (int i = 1; i < n;)
        {
            a = b;
            if ((a - path[i]).norm() > progress)
            {
                b = (path[i] - a).normalized() * progress + a;
            }
            else
            {
                b = path[i];
                i++;
            }
            bs.emplace_back(b);

            bd(0, 3) = -std::min(std::max(a(0), b(0)) + range, highCorner(0));
            bd(1, 3) = +std::max(std::min(a(0), b(0)) - range, lowCorner(0));
            bd(2, 3) = -std::min(std::max(a(1), b(1)) + range, highCorner(1));
            bd(3, 3) = +std::max(std::min(a(1), b(1)) - range, lowCorner(1));
            bd(4, 3) = -std::min(std::max(a(2), b(2)) + range, highCorner(2));
            bd(5, 3) = +std::max(std::min(a(2), b(2)) - range, lowCorner(2));

            valid_pc.clear();
            for (const Eigen::Vector3d &p : points)
            {
                if ((bd.leftCols<3>() * p + bd.rightCols<1>()).maxCoeff() < 0.0)
                {
                    valid_pc.emplace_back(p);
                }
            }
            Eigen::Map<const Eigen::Matrix<double, 3, -1, Eigen::ColMajor>> pc(valid_pc[0].data(), 3, valid_pc.size());

            firi::firi(bd, pc, a, b, hp);

            if (hpolys.size() != 0)
            {
                const Eigen::Vector4d ah(a(0), a(1), a(2), 1.0);
                if (3 <= ((hp * ah).array() > -eps).cast<int>().sum() +
                             ((hpolys.back() * ah).array() > -eps).cast<int>().sum())
                {
                    firi::firi(bd, pc, a, a, gap, 1);
                    hpolys.emplace_back(gap);
                }
            }

            hpolys.emplace_back(hp);
        }
    }

    inline void shortCut(std::vector<Eigen::MatrixX4d> &hpolys)
    {
        std::vector<Eigen::MatrixX4d> htemp = hpolys;
        if (htemp.size() == 1)
        {
            Eigen::MatrixX4d headPoly = htemp.front();
            htemp.insert(htemp.begin(), headPoly);
        }
        hpolys.clear();

        int M = htemp.size();
        Eigen::MatrixX4d hPoly;
        bool overlap;
        std::deque<int> idices;
        idices.push_front(M - 1);
        for (int i = M - 1; i >= 0; i--)
        {
            for (int j = 0; j < i; j++)
            {
                if (j < i - 1)
                {
                    overlap = geo_utils::overlap(htemp[i], htemp[j], 0.01);
                }
                else
                {
                    overlap = true;
                }
                if (overlap)
                {
                    idices.push_front(j);
                    i = j + 1;
                    break;
                }
            }
        }
        for (const auto &ele : idices)
        {
            hpolys.push_back(htemp[ele]);
        }
    }

}

#endif


#include <cassert>      // assert
#include <iterator>     // iterator
#include <utility>      // swap
#include <Eigen/Dense>

#include "partition.hpp"

template <class uint>
class PartitionList {
	private:
		class PartitionIterator : std::iterator< std::bidirectional_iterator_tag, Partition<uint> > {
			private:
				Partition<uint>* current;
				bool invalid;

			public:
				PartitionIterator() : current(nullptr), invalid(true) {}
				PartitionIterator(const PartitionIterator& other) : invalid(other.invalid) {
					current = new Partition<uint>(*other);
				}
				PartitionIterator(const Partition<uint>& other) : invalid(false) {
					current = new Partition<uint>(other);
				}
				~PartitionIterator() {
					if (current != nullptr)
						delete current;
				}

				void swap(PartitionIterator& other) noexcept {
					std::swap(invalid, other.invalid);
					std::swap(current, other.current);
				}

				PartitionIterator& operator++() {
					assert(current != nullptr);
					assert(!invalid);
					if (!current->next())
						invalid = true;

					return *this;
				}
				PartitionIterator& operator--() {
					assert(current != nullptr);
					assert(!current->isMin());
					if (invalid) {
						current->setMax();
						invalid = false;
					}
					else
						current->prev();
					return *this;
				}

				PartitionIterator operator++(int) {
					PartitionIterator response(*this);
					this->operator++();
					return response;
				}
				PartitionIterator operator--(int) {
					PartitionIterator response(*this);
					this->operator--;
					return response;
				}

				bool operator==(const PartitionIterator& rhs) const {
					return (invalid && rhs.invalid) || (!(invalid || rhs.invalid) && *current == *(rhs.current));
				}
				bool operator!=(const PartitionIterator& rhs) const {
					return (invalid != rhs.invalid) || (!(invalid || rhs.invalid) && *current != *(rhs.current));
				}
				bool operator<(const PartitionIterator& rhs) const {
					if (invalid) {
						return false;
					}
					if (rhs.invalid) {
						return true;
					}
					return *current < *(rhs.current);
				}
				bool operator<=(const PartitionIterator& rhs) const {
					if (invalid) {
						return rhs.invalid;
					}
					if (rhs.invalid) {
						return true;
					}
					return *current <= *(rhs.current);
				}
				bool operator>(const PartitionIterator& rhs) const {
					return !(*this <= rhs);
				}
				bool operator>=(const PartitionIterator& rhs) const {
					return !(*this < rhs);
				}

				const Partition<uint>& operator*() const {
					return *current;
				}
				const Partition<uint>& operator->() const {
					return *current;
				}

		};

		PartitionIterator *m_first, *m_last;

		Eigen::Array<uint, Eigen::Dynamic, Eigen::Dynamic> sizes;

		// helper function for finding the partition offset
		uint getIndex(const Partition<uint>& n) const {
			uint index = 0;

			uint k = n.getLength();
			uint m = n.getMass();

			for (uint i = k; i > 2; i--) {
				for (uint j = 0; j < n.get(i); j++) {
					index += sizes(m - i*j, i-3);
				}
				m -= i * n.get(i);
			}
			index += n.get(2);
			return index;
		}
	public:
		typedef PartitionIterator iterator;
		typedef PartitionIterator const_iterator;

		PartitionList() : m_first(nullptr), m_last(nullptr) {}
		PartitionList(size_t len, uint mass) {
			Partition<uint> a(len, mass);
		  	m_first = new PartitionIterator(a);
			a.setMax();
			m_last = new PartitionIterator(a);
			++(*m_last);

			uint num = (uint) (len*(len+1)/2+1);
			Eigen::Array<uint, Eigen::Dynamic, 1> c;
			c = decltype(c)::Zero(num);
			sizes = decltype(sizes)::Zero(mass+1, len-1);
			sizes.row(0).setConstant(1);
			sizes.col(0) = decltype(c)::LinSpaced(mass+1,0,mass) / 2 + 1;
			c(0) = 1; c(1) = -1; c(2) = -1; c(3) = 1;

			for (size_t i = 3; i <= len; i++) {
				uint cnum = (uint) (i*(i+1)/2);
				c.segment(i, cnum+1-i) -= c.head(cnum+1-i).eval();

				if (cnum > mass) {
					for (uint j = 1; j <= mass; j++)
						sizes(j, i-2) = -c.segment(1, j).reverse().matrix().dot(sizes.col(i-2).head(j).matrix());
				}
				else {
					for (uint j = 1; j < cnum; j++)
						sizes(j, i-2) = -c.segment(1, j).reverse().matrix().dot(sizes.col(i-2).head(j).matrix());
					for (uint j = cnum; j <= mass; j++)
						sizes(j, i-2) = -c.segment(1, cnum).reverse().matrix().dot(sizes.col(i-2).segment(j-cnum,cnum).matrix());
				}
			}

		}
		~PartitionList() {
			if (m_first != nullptr)
				delete m_first;
			if (m_last != nullptr)
				delete m_last;
		}

		uint indexOf(const Partition<uint>& n) const {
			uint index = 0;
			uint m = n.getMass();
			for (uint i = n.getLength(); i > 2; i--) {
				for (uint j = 0; j < n.get(i); j++) {
					index += sizes(m - i*j, i-3);
				}
				m -= i * n.get(i);
			}
			index += n.get(2);
			return index;
		}
		uint size() const {
			uint mass = (**m_first).getMass();
			uint len = (**m_first).getLength();
			return sizes(mass, len-2);
		}

		const_iterator begin() const {
			return iterator(*m_first);
		}
		const_iterator end() const {
			return iterator(*m_last);
		}
		const_iterator cbegin() const {
			return const_iterator(*m_first);
		}
		const_iterator cend() const {
			return const_iterator(*m_last);
		}
};


//  (C) Copyright Nick Thompson 2018.
//  Use, modification and distribution are 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)

#ifndef BOOST_MATH_TOOLS_UNIVARIATE_STATISTICS_HPP
#define BOOST_MATH_TOOLS_UNIVARIATE_STATISTICS_HPP

#include <algorithm>
#include <iterator>
#include <tuple>
#include <boost/assert.hpp>
#include <boost/config/header_deprecated.hpp>

BOOST_HEADER_DEPRECATED("<boost/math/statistics/univariate_statistics.hpp>");

namespace boost::math::tools {

template<class ForwardIterator>
auto mean(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");
    if constexpr (std::is_integral<Real>::value)
    {
        double mu = 0;
        double i = 1;
        for(auto it = first; it != last; ++it) {
            mu = mu + (*it - mu)/i;
            i += 1;
        }
        return mu;
    }
    else if constexpr (std::is_same_v<typename std::iterator_traits<ForwardIterator>::iterator_category, std::random_access_iterator_tag>)
    {
        size_t elements = std::distance(first, last);
        Real mu0 = 0;
        Real mu1 = 0;
        Real mu2 = 0;
        Real mu3 = 0;
        Real i = 1;
        auto end = last - (elements % 4);
        for(auto it = first; it != end;  it += 4) {
            Real inv = Real(1)/i;
            Real tmp0 = (*it - mu0);
            Real tmp1 = (*(it+1) - mu1);
            Real tmp2 = (*(it+2) - mu2);
            Real tmp3 = (*(it+3) - mu3);
            // please generate a vectorized fma here
            mu0 += tmp0*inv;
            mu1 += tmp1*inv;
            mu2 += tmp2*inv;
            mu3 += tmp3*inv;
            i += 1;
        }
        Real num1 = Real(elements  - (elements %4))/Real(4);
        Real num2 = num1 + Real(elements % 4);

        for (auto it = end; it != last; ++it)
        {
            mu3 += (*it-mu3)/i;
            i += 1;
        }

        return (num1*(mu0+mu1+mu2) + num2*mu3)/Real(elements);
    }
    else
    {
        auto it = first;
        Real mu = *it;
        Real i = 2;
        while(++it != last)
        {
            mu += (*it - mu)/i;
            i += 1;
        }
        return mu;
    }
}

template<class Container>
inline auto mean(Container const & v)
{
    return mean(v.cbegin(), v.cend());
}

template<class ForwardIterator>
auto variance(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute mean and variance.");
    // Higham, Accuracy and Stability, equation 1.6a and 1.6b:
    if constexpr (std::is_integral<Real>::value)
    {
        double M = *first;
        double Q = 0;
        double k = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            double tmp = *it - M;
            Q = Q + ((k-1)*tmp*tmp)/k;
            M = M + tmp/k;
            k += 1;
        }
        return Q/(k-1);
    }
    else
    {
        Real M = *first;
        Real Q = 0;
        Real k = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            Real tmp = (*it - M)/k;
            Q += k*(k-1)*tmp*tmp;
            M += tmp;
            k += 1;
        }
        return Q/(k-1);
    }
}

template<class Container>
inline auto variance(Container const & v)
{
    return variance(v.cbegin(), v.cend());
}

template<class ForwardIterator>
auto sample_variance(ForwardIterator first, ForwardIterator last)
{
    size_t n = std::distance(first, last);
    BOOST_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(first, last)/(n-1);
}

template<class Container>
inline auto sample_variance(Container const & v)
{
    return sample_variance(v.cbegin(), v.cend());
}

// Follows equation 1.5 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ForwardIterator>
auto skewness(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute skewness.");
    if constexpr (std::is_integral<Real>::value)
    {
        double M1 = *first;
        double M2 = 0;
        double M3 = 0;
        double n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            double delta21 = *it - M1;
            double tmp = delta21/n;
            M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 = M2 + tmp*(n-1)*delta21;
            M1 = M1 + tmp;
            n += 1;
        }

        double var = M2/(n-1);
        if (var == 0)
        {
            // The limit is technically undefined, but the interpretation here is clear:
            // A constant dataset has no skewness.
            return double(0);
        }
        double skew = M3/(M2*sqrt(var));
        return skew;
    }
    else
    {
        Real M1 = *first;
        Real M2 = 0;
        Real M3 = 0;
        Real n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            Real delta21 = *it - M1;
            Real tmp = delta21/n;
            M3 += tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 += tmp*(n-1)*delta21;
            M1 += tmp;
            n += 1;
        }

        Real var = M2/(n-1);
        if (var == 0)
        {
            // The limit is technically undefined, but the interpretation here is clear:
            // A constant dataset has no skewness.
            return Real(0);
        }
        Real skew = M3/(M2*sqrt(var));
        return skew;
    }
}

template<class Container>
inline auto skewness(Container const & v)
{
    return skewness(v.cbegin(), v.cend());
}

// Follows equation 1.5/1.6 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ForwardIterator>
auto first_four_moments(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute the first four moments.");
    if constexpr (std::is_integral<Real>::value)
    {
        double M1 = *first;
        double M2 = 0;
        double M3 = 0;
        double M4 = 0;
        double n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            double delta21 = *it - M1;
            double tmp = delta21/n;
            M4 = M4 + tmp*(tmp*tmp*delta21*((n-1)*(n*n-3*n+3)) + 6*tmp*M2 - 4*M3);
            M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 = M2 + tmp*(n-1)*delta21;
            M1 = M1 + tmp;
            n += 1;
        }

        return std::make_tuple(M1, M2/(n-1), M3/(n-1), M4/(n-1));
    }
    else
    {
        Real M1 = *first;
        Real M2 = 0;
        Real M3 = 0;
        Real M4 = 0;
        Real n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            Real delta21 = *it - M1;
            Real tmp = delta21/n;
            M4 = M4 + tmp*(tmp*tmp*delta21*((n-1)*(n*n-3*n+3)) + 6*tmp*M2 - 4*M3);
            M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 = M2 + tmp*(n-1)*delta21;
            M1 = M1 + tmp;
            n += 1;
        }

        return std::make_tuple(M1, M2/(n-1), M3/(n-1), M4/(n-1));
    }
}

template<class Container>
inline auto first_four_moments(Container const & v)
{
    return first_four_moments(v.cbegin(), v.cend());
}

// Follows equation 1.6 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ForwardIterator>
auto kurtosis(ForwardIterator first, ForwardIterator last)
{
    auto [M1, M2, M3, M4] = first_four_moments(first, last);
    if (M2 == 0)
    {
        return M2;
    }
    return M4/(M2*M2);
}

template<class Container>
inline auto kurtosis(Container const & v)
{
    return kurtosis(v.cbegin(), v.cend());
}

template<class ForwardIterator>
auto excess_kurtosis(ForwardIterator first, ForwardIterator last)
{
    return kurtosis(first, last) - 3;
}

template<class Container>
inline auto excess_kurtosis(Container const & v)
{
    return excess_kurtosis(v.cbegin(), v.cend());
}

template<class RandomAccessIterator>
auto median(RandomAccessIterator first, RandomAccessIterator last)
{
    size_t num_elems = std::distance(first, last);
    BOOST_ASSERT_MSG(num_elems > 0, "The median of a zero length vector is undefined.");
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last);
        return *middle;
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last);
        std::nth_element(middle, middle+1, last);
        return (*middle + *(middle+1))/2;
    }
}

template<class RandomAccessContainer>
inline auto median(RandomAccessContainer & v)
{
    return median(v.begin(), v.end());
}

template<class RandomAccessIterator>
auto gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    BOOST_ASSERT_MSG(first != last && std::next(first) != last, "Computation of the Gini coefficient requires at least two samples.");

    std::sort(first, last);
    if constexpr (std::is_integral<Real>::value)
    {
        double i = 1;
        double num = 0;
        double denom = 0;
        for (auto it = first; it != last; ++it)
        {
            num += *it*i;
            denom += *it;
            ++i;
        }

        // If the l1 norm is zero, all elements are zero, so every element is the same.
        if (denom == 0)
        {
            return double(0);
        }

        return ((2*num)/denom - i)/(i-1);
    }
    else
    {
        Real i = 1;
        Real num = 0;
        Real denom = 0;
        for (auto it = first; it != last; ++it)
        {
            num += *it*i;
            denom += *it;
            ++i;
        }

        // If the l1 norm is zero, all elements are zero, so every element is the same.
        if (denom == 0)
        {
            return Real(0);
        }

        return ((2*num)/denom - i)/(i-1);
    }
}

template<class RandomAccessContainer>
inline auto gini_coefficient(RandomAccessContainer & v)
{
    return gini_coefficient(v.begin(), v.end());
}

template<class RandomAccessIterator>
inline auto sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    size_t n = std::distance(first, last);
    return n*gini_coefficient(first, last)/(n-1);
}

template<class RandomAccessContainer>
inline auto sample_gini_coefficient(RandomAccessContainer & v)
{
    return sample_gini_coefficient(v.begin(), v.end());
}

template<class RandomAccessIterator>
auto median_absolute_deviation(RandomAccessIterator first, RandomAccessIterator last, typename std::iterator_traits<RandomAccessIterator>::value_type center=std::numeric_limits<typename std::iterator_traits<RandomAccessIterator>::value_type>::quiet_NaN())
{
    using std::abs;
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    using std::isnan;
    if (isnan(center))
    {
        center = boost::math::tools::median(first, last);
    }
    size_t num_elems = std::distance(first, last);
    BOOST_ASSERT_MSG(num_elems > 0, "The median of a zero-length vector is undefined.");
    auto comparator = [&center](Real a, Real b) { return abs(a-center) < abs(b-center);};
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last, comparator);
        return abs(*middle);
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last, comparator);
        std::nth_element(middle, middle+1, last, comparator);
        return (abs(*middle) + abs(*(middle+1)))/abs(static_cast<Real>(2));
    }
}

template<class RandomAccessContainer>
inline auto median_absolute_deviation(RandomAccessContainer & v, typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(v.begin(), v.end(), center);
}

}
#endif


#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

/*
Test Suite for method Route::minElevation() - N0680788
--------------------------------------------------------
The original set position that I have used for all the tests is Earth::CityCampus, which has a set elevation of 53 metres.

The test cases that I have designed are:

 - null_elevation:
This will test a route that has been set with null elevation (constant) when using the GridWorld constructor, the value returned should be equal to the set position (53).
 - constant_elevation:
This will test a route that has a constant elevation i.e. GHI.
 - elevation_above_sealevel:
This will test a route that starts below sea level i.e. negative elevation, then proceeds to increase in elevation above sea level to the original position (53).
 - elevation_below_sealevel:
This will test a route that starts above sea level i.e. original position, then proceeds to decrease in elevation below sea level to a negative value.
 - increasing_elevation:
This will test a route that starts at a position on the outer level of gridworld, and is positive, i.e. ABCDE. The route will increase in elevation to the original set position (53).
 - decreasing_elevation:
This will test a route that starts at the original set position (53), and decreases to a position on the outer level of gridworld, and is positive.
 - pyramid_elevation:
This will test a route starting from a elevation on the outer level of gridworld, increasing to the original set elevation (53), and then back to the start elevation.
 - valley_elevation:
This will test a route starting from the original set elevation (53), then decreases to a elevation on the outer level of gridworld, then back to the original set elevation.
*/

BOOST_AUTO_TEST_SUITE( n0680788_Route_minElevation )

const bool correctFile = true;

//The route has been set is 'MHC'
//The GridWorld produced has been set so that the difference in elevation is 0.
//This means that for any route given, the elevation is not going to change (constant)
//Meaning that the minimum elevation is going to be the original set position, which should return 53 metres.
BOOST_AUTO_TEST_CASE( null_elevation )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_nullElevationDifference.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), 53.000000 );
}

//The route that has been set is 'GHI'
//The GridWorld produced has been set so that the difference in elevation is 20.
//This means that because GHI all have the same elevation, there would be no change, but because they are on a lower level than the original set elevation, the value would different to 53.
//Therefore the minimum value returned should have a difference of 20 from the original set elevation. In this case 33.
BOOST_AUTO_TEST_CASE( constant_elevation )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_constantElevation.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), 33.000000 );
}

//The route that has been set is 'CHM'
//The GridWorld produced has been set so that the difference in elevation is 30.
//The reason for this is so that a position on the lowest level of GridWorld, has a negative elevation which would be below sea level.
//A position 'C' on the lowest level would be the start position, and the end of the route would be the original set position i.e. CityCampus.
//Since as 'C' is 2 levels away from 'M' (CityCampus), the minimum elevation value should be -7 (53-2(30))
BOOST_AUTO_TEST_CASE( elevation_above_sealevel )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_riseAboveSeaLevel.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), -7.000000 );
}

//The route that has been set is 'MHC'
//The GridWorld produced has been set so that the difference in elevation is 30.
//Similar to 'elevation_above_sealevel', a position on the outer level of GridWorld would have a negative elevation.
//So the start position 'M' (CityCampus)  would be the highest elevation, and the last position C would have the lowest position elevation.
//Since as 'C' is 2 levels away from 'M' (CityCampus), the minimum elevation value should be -7 (53-2(30))
BOOST_AUTO_TEST_CASE( elevation_below_sealevel )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_fallBelowSeaLevel.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), -7.000000 );
}

//The route that has been set is 'CHM'
//The GridWorld produced has been set so that the difference in elevation is 20.
//This route will start on the lowest elevation based on elevation difference, and increase in elevation to the elevation of CityCampus.
//Since as the difference in elevation is 20, the minimum elevation value returned should be 13 (53-2(20))
BOOST_AUTO_TEST_CASE( increasing_elevation )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_increasingElevation.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), 13.000000 );
}

//The route that has been set is 'MHC'
//The GridWorld produced has been set so that the difference in elevation is 20.
//The start position would be CityCampus, and the elevation should decrease to the lowest elevation based on the elevation difference.
//Since as the difference in elevation is 20, the minimum elevation value returned should be 13 (53-2(20))
BOOST_AUTO_TEST_CASE( decreasing_elevation )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_decreasingElevation.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), 13.000000 );
}

//The route that has been set is 'CHMHC'
//The GridWorld produced has been set so that the difference in elevation is 20.
//The reason for the elevation being given in a pyramid shape, is due to the start postion being set to 'C' which is the lowest possible elevation value based on elevation difference, and then increasing to 'M' which is the CityCampus position (highest elevation), then going back down to 'C'
//Since as the difference in elevation is 20, the minimum elevation value returned should be 13 (53-2(20))
BOOST_AUTO_TEST_CASE( pyramid_elevation )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_pyramid.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), 13.000000 );
}

//The route that has been set is 'MHCHC'
//The GridWorld produced has been set so that the difference in elevation is 20.
//The reason for the elevation being given in a valley shape, is due to the start position being set to 'M' CityCampus and then decreasing to the lowest possible elevation 'C' and then increasing back to 'M'.
//Since as the difference in elevation is 20, the minimum elevation value returned should be 13 (53-2(20))
BOOST_AUTO_TEST_CASE( valley_elevation )
{
   Route route = Route(LogFiles::GPXRoutesDir + "minElevation_n0680788_valley.gpx", correctFile);
   BOOST_CHECK_EQUAL( route.minElevation(), 13.000000 );
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_DYN_LINK

#define BOOST_TEST_MODULE algorithm/crossover

#include <boost/test/unit_test.hpp>

#include <set>
#include <vector>

#include <geneial/algorithm/BaseGeneticAlgorithm.h>
#include <geneial/core/population/chromosome/MultiValueChromosome.h>

#include <geneial/core/population/builder/ContinousMultiValueChromosomeFactory.h>
#include <geneial/core/population/builder/ContinousMultiValueBuilderSettings.h>

#include <geneial/core/operations/crossover/MultiValueChromosomeBlendingCrossover.h>

#include "mocks/MockFitnessEvaluator.h"

#include "helper/common_casts.h"
#include "helper/continuity.h"
#include "helper/chromosome_compare.h"

using ::geneial::operation::crossover::MultiValueChromosomeBlendingCrossover;

using InterpolateBeta = ::geneial::operation::crossover::MultiValueChromosomeBlendingCrossover<int,double>::InterpolateBeta;

struct mvc_blending_testsets
{
    InterpolateBeta interpolationMethod;
    int eps;
    int num_vals;
    int random_min;
    int random_max;
    bool hasStart;
    int start;

};
const int CONTINUITY_BLENDING_SIGNIFICANCE = 1000;
BOOST_AUTO_TEST_SUITE( __CROSSOVER_MVCBlendingCrossover )
BOOST_AUTO_TEST_CASE( ContinuityBlending )
{
    const test_mock::MockFitnessEvaluator<double>::ptr evaluator(new test_mock::MockFitnessEvaluator<double>);

       std::vector<mvc_blending_testsets> blending_testsets = {
               { InterpolateBeta::INTERPOLATE_LINEARLY, 1, 50, 0, 100, true, 1},
               { InterpolateBeta::INTERPOLATE_LINEARLY, 1, 50, 0, 100, true, 100},
               { InterpolateBeta::INTERPOLATE_LINEARLY, 2, 50, 0, 100, false,0},
               { InterpolateBeta::INTERPOLATE_LINEARLY, 2, 50, -100, 100, false, 0},
               { InterpolateBeta::INTERPOLATE_LINEARLY, 2, 50, -100, 0, false, 0},
               { InterpolateBeta::INTERPOLATE_LINEARLY, 10, 5, 0, 10, false, 0},
               { InterpolateBeta::INTERPOLATE_LINEARLY, 10, 5, 0, 10, true, 5},

               { InterpolateBeta::INTERPOLATE_NONE, 1, 50, 0, 100, true, 1},
               { InterpolateBeta::INTERPOLATE_NONE, 1, 50, 0, 100, true, 100},
               { InterpolateBeta::INTERPOLATE_NONE, 2, 50, 0, 100, false,0},
               { InterpolateBeta::INTERPOLATE_NONE, 2, 50, -100, 100, false, 0},
               { InterpolateBeta::INTERPOLATE_NONE, 2, 50, -100, 0, false, 0},
               { InterpolateBeta::INTERPOLATE_NONE, 10, 5, 0, 10, false, 0},
               { InterpolateBeta::INTERPOLATE_NONE, 10, 5, 0, 10, true, 5},

               { InterpolateBeta::INTERPOLATE_COSINE, 1, 50, 0, 100, true, 1},
               { InterpolateBeta::INTERPOLATE_COSINE, 1, 50, 0, 100, true, 100},
               { InterpolateBeta::INTERPOLATE_COSINE, 2, 50, 0, 100, false,0},
               { InterpolateBeta::INTERPOLATE_COSINE, 2, 50, -100, 100, false, 0},
               { InterpolateBeta::INTERPOLATE_COSINE, 2, 50, -100, 0, false, 0},
               { InterpolateBeta::INTERPOLATE_COSINE, 10, 5, 0, 10, false, 0},
               { InterpolateBeta::INTERPOLATE_COSINE, 10, 5, 0, 10, true, 5},

               { InterpolateBeta::INTERPOLATE_RANDOM, 1, 50, 0, 100, true, 1},
               { InterpolateBeta::INTERPOLATE_RANDOM, 1, 50, 0, 100, true, 100},
               { InterpolateBeta::INTERPOLATE_RANDOM, 2, 50, 0, 100, false,0},
               { InterpolateBeta::INTERPOLATE_RANDOM, 2, 50, -100, 100, false, 0},
               { InterpolateBeta::INTERPOLATE_RANDOM, 2, 50, -100, 0, false, 0},
               { InterpolateBeta::INTERPOLATE_RANDOM, 10, 5, 0, 10, false, 0},
               { InterpolateBeta::INTERPOLATE_RANDOM, 10, 5, 0, 10, true, 5}
       };

       int i = CONTINUITY_BLENDING_SIGNIFICANCE;

       while(i--)
       {
           for(const auto& testset : blending_testsets)
           {
               auto builder = ContinousMultiValueChromosomeFactory<int,double>::Builder(evaluator);

               auto factory = std::dynamic_pointer_cast<ContinousMultiValueChromosomeFactory<int,double>>(builder.create());

               builder.getSettings().setNum(testset.num_vals);
               builder.getSettings().setEps(testset.eps);
               builder.getSettings().setRandomMin(testset.random_min);
               builder.getSettings().setRandomMax(testset.random_max);
               builder.getSettings().setHasStart(testset.hasStart);
               builder.getSettings().setStartValue(testset.start);

               auto chromosomeA = factory->createChromosome();
               auto chromosomeB = factory->createChromosome();

               int trials = 100;
               while(trials-- && !test_helper::chromosomeCompare<int,double>(chromosomeA,chromosomeB))
               {
                   chromosomeB = factory->createChromosome();
               }
               const bool compareDifferentAB = test_helper::chromosomeCompare<int,double>(chromosomeA,chromosomeB);
               BOOST_ASSERT(!compareDifferentAB);

//               std::cout << *chromosomeA <<std::endl;
//               std::cout << *chromosomeB <<std::endl;

               test_helper::checkContinuity<int,double>(chromosomeA,testset.eps);
               test_helper::checkContinuity<int,double>(chromosomeB,testset.eps);

               auto crossoverOperationBuilder = ::geneial::operation::crossover::MultiValueChromosomeBlendingCrossover<int,double>::Builder(factory);
               crossoverOperationBuilder.setNumChilds(1);
               crossoverOperationBuilder.setInterpolationMethod(testset.interpolationMethod);
               auto operation = crossoverOperationBuilder.create();

               auto childcontainer = operation->doCrossover(chromosomeA,chromosomeB);
               BOOST_CHECK(childcontainer.size() == 1);

               //differs from mommie/daddy?
               const bool compareDifferentA = test_helper::chromosomeCompare<int,double>(childcontainer[0],chromosomeA);
               BOOST_ASSERT(!compareDifferentA);
               const bool compareDifferentB = test_helper::chromosomeCompare<int,double>(childcontainer[0],chromosomeB);
               BOOST_ASSERT(!compareDifferentB);

               if(testset.interpolationMethod != InterpolateBeta::INTERPOLATE_NONE)
               {
                   //Continuity preserving?
                   //TODO (bewo): Bug, Blending does not preserve continuity.
                   test_helper::checkContinuity<int,double>(childcontainer[0],testset.eps);
                   //std::cout << *childcontainer[0] << std::endl;
                   if(testset.hasStart)
                   {
                       //TODO (bewo): Blending ignores start value
                       //test_helper::checkForStart<int,double>(childcontainer[0],testset.start,testset.eps);
                   }
               }
               test_helper::checkLimits<int,double>(childcontainer[0],testset.random_min,testset.random_max);
           }
       }

}
BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include "SMT_Graph.hpp"
#include "SMT_Tag_Mapping.hpp"
#include <boost/utility/enable_if.hpp>
#include <boost/foreach.hpp>
#include <boost/dynamic_bitset.hpp>

#include <ostream>
#include <set>

namespace metaSMT {

  namespace predtags = ::metaSMT::logic::tag;
  namespace bvtags = ::metaSMT::logic::QF_BV::tag;
  namespace arraytags = ::metaSMT::logic::Array::tag;
  namespace mpl = boost::mpl;

  void print_SMT_Expression(std::ostream &outfile,
                            SMT_Graph const &g,
                            SMT_Expression const &v);

  struct Vertex_Printer : public boost::static_visitor<void> {
  public:
    Vertex_Printer(std::ostream &outfile,
                   SMT_Graph const &g,
                   SMT_Expression const &v)
      : outfile_(outfile),
        g_(g), v_(v)
    {}

    inline void operator()(bvtags::zero_extend_tag const &tag) const {
      boost::any arg = boost::get(boost::vertex_arg, g_, v_);
      unsigned long width = boost::any_cast<unsigned long>(arg);
      outfile_ << "(zero_extend[" << width << "] ";
      BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
        print_SMT_Expression(outfile_, g_, target(e, g_));
      }
      outfile_ << ')';
    }

    inline void operator()(bvtags::bvbin_tag const &tag) const {
      boost::any arg = boost::get(boost::vertex_arg, g_, v_);
      std::string bin_const = boost::any_cast<std::string>(arg);
      outfile_ << "(bvbin" << bin_const << ")";
    }

    inline void operator()(bvtags::bvhex_tag const &tag) const {
      boost::any arg = boost::get(boost::vertex_arg, g_, v_);
      std::string hex_const = boost::any_cast<std::string>(arg);
      outfile_ << "(bvhex" << hex_const << ")";
    }

    inline void operator()(bvtags::bvsint_tag const &tag) const {
      typedef boost::tuple<long, unsigned long> tuple_type;
      boost::any arg = boost::get(boost::vertex_arg, g_, v_);
      tuple_type const tuple = boost::any_cast<tuple_type>(arg);
      unsigned long const width = tuple.get<1>();
      boost::dynamic_bitset<> bv(width, tuple.get<0>());
      std::string s; to_string(bv, s);
      outfile_ << "(bvbin" << s << ")";
    }

    inline void operator()(bvtags::bvuint_tag const &tag) const {
      typedef boost::tuple<unsigned long, unsigned long> tuple_type;
      boost::any arg = boost::get(boost::vertex_arg, g_, v_);
      tuple_type tuple = boost::any_cast<tuple_type>(arg);
      outfile_ << "(bv" << tuple.get<0>() << "[" << tuple.get<1>() << "])";
    }

    inline void operator()(bvtags::extract_tag const &tag) const {
      typedef boost::tuple<unsigned long, unsigned long> tuple_type;
      boost::any arg = boost::get(boost::vertex_arg, g_, v_);
      tuple_type tuple = boost::any_cast<tuple_type>(arg);
      outfile_ << "(extract[" << tuple.get<0>() << ':' << tuple.get<1>() << "] ";
      BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
        print_SMT_Expression(outfile_, g_, target(e, g_));
      }
      outfile_ << ")";
    }

    inline void operator()(predtags::nequal_tag const &tag) const {
      outfile_ << "(not (=";
      BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
        print_SMT_Expression(outfile_, g_, target(e, g_));
      }
      outfile_ << "))";
    }

    inline void operator()(predtags::distinct_tag const &tag) const {
      outfile_ << "(distinct ";
      BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
        print_SMT_Expression(outfile_, g_, target(e, g_));
      }
      outfile_ << ")";
    }

    inline void operator()(predtags::nand_tag const &tag) const {
      outfile_ << "(not (and";
      BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
        print_SMT_Expression(outfile_, g_, target(e, g_));
      }
      outfile_ << "))";
    }

    inline void operator()(predtags::nor_tag const &tag) const {
      outfile_ << "(not (or";
      BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
        print_SMT_Expression(outfile_, g_, target(e, g_));
      }
      outfile_ << "))";
    }

    inline void operator()(predtags::xnor_tag const &tag) const {
      outfile_ << "(not (xor";
      BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
        print_SMT_Expression(outfile_, g_, target(e, g_));
      }
      outfile_ << "))";
    }

    inline void operator()(predtags::var_tag const &tag) const {
      outfile_ << "p_" << tag.id;
    }

    inline void operator()(bvtags::var_tag const &tag) const {
      outfile_ << "bitv_" << tag.id;
    }

    inline void operator()(arraytags::array_var_tag const &tag) const {
      outfile_ << "array_" << tag.id;
    }

    template <typename T>
    inline typename boost::enable_if<
      typename mpl::has_key< SMT_NameMap, T>::type
    >::type
    operator()(T const &t) const {
      typedef typename mpl::at< SMT_NameMap, T >::type name;

      if (out_degree(v_, g_) != 0) {
        outfile_ << '(' << mpl::c_str<name>::value;
        BOOST_FOREACH (SMT_Edge e, out_edges(v_, g_)) {
          outfile_ << ' ';
          print_SMT_Expression(outfile_, g_, target(e, g_));
        }
        outfile_ << ')';
      } else {
        outfile_ << mpl::c_str<name>::value;
      }
    }

    template <typename T>
    inline typename boost::disable_if<
      typename mpl::has_key< SMT_NameMap, T>::type
    >::type
    operator()(T const &t) const {
      outfile_ << "(MISSING " << t <<")";
    }

  private:
    std::ostream &outfile_;
    SMT_Graph const &g_;
    SMT_Expression const &v_;
  };

  inline void print_SMT_Expression(std::ostream & outfile,
                                   SMT_Graph const &g,
                                   SMT_Expression const &v) {
    outfile << ' ';
    metaSMT::Tag tag = boost::get(boost::vertex_tag, g, v);
    boost::apply_visitor(Vertex_Printer(outfile, g, v), tag);
  }

  struct SMT_Declaration_Printer : public boost::static_visitor<void> {
    SMT_Declaration_Printer(std::ostream &outfile,
                            SMT_Graph const &g,
                            SMT_Expression const &v)
      : outfile_(outfile),
        g_(g), v_(v)
    {}

    inline void operator()(predtags::var_tag const &tag) const {
      outfile_ << " :extrapreds ((" << "p_" << tag.id << "))\n";
    }

    inline void operator()(bvtags::var_tag const &tag) const {
      outfile_ << " :extrafuns ((" << "bitv_" << tag.id
               << " BitVec[" << tag.width << "]))\n";
    }

    inline void operator()(arraytags::array_var_tag const &tag) const {
      outfile_ << " :extrafuns ((" << "array_" << tag.id
               << " Array[" << tag.index_width << ':' << tag.elem_width << "]))\n";
    }

    template <typename T>
    inline void operator()(T const &t) const
    {}

  private:
    std::ostream &outfile_;
    SMT_Graph const &g_;
    SMT_Expression const &v_;
  };

  inline void write_smt(std::ostream & outfile,
                        SMT_Graph const & g,
                        std::vector<SMT_Expression> const &assertions) {
    outfile
      << "(benchmark metaSMT.smt\n"
      << " :source { Generated by metaSMT }\n"
      << " :status unknown\n"
      << " :difficulty unknown\n"
      << " :logic QF_BV\n"
    ;

    BOOST_FOREACH (SMT_Expression v, vertices(g)) {
      metaSMT::Tag tag = boost::get(boost::vertex_tag, g, v);
      boost::apply_visitor(SMT_Declaration_Printer(outfile, g, v), tag);
    }

    BOOST_FOREACH (SMT_Expression v, assertions) {
      outfile << " :assumption ";
      print_SMT_Expression(outfile, g, v);
      outfile << '\n'; // assertion
    }

    // outfile << " :formula true\n";

    outfile << ")\n"; // benchmark
  }
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      111117    L. van der Ham    File created.
 *      111122    K. Kumar          Minor comment changes; include-statements updated, updated
 *                                  tests.
 *      120306    L. van der Ham    Correction of some small typos.
 *      120307    K. Kumar          Boostified unit tests; moved file.
 *
 *    References
 *      Koon, W.S., et al. Dynamical Systems, the Three-Body Problem and Space Mission Design,
 *          2006.
 *      Jet Propulsion Laboratory, NASA. Planets and Pluto: Physical Characteristics.
 *          http://ssd.jpl.nasa.gov/?planet_phys_par, last updated: 5th November, 2008, last
 *          accessed: 22nd November, 2011.
 *      Jet Propulsion Laboratory, NASA. Astrodynamics Constants.
 *          http://ssd.jpl.nasa.gov/?constants, last updated: 6th September, 2011, last accessed:
 *          22nd November, 2011.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/Gravitation/unitConversionsCircularRestrictedThreeBodyProblem.h"

namespace tudat
{
namespace unit_tests
{

//! Test if Cartesian state conversion for CRTBP is computed correctly.
BOOST_AUTO_TEST_CASE( testCartesianStateConversionCircularRestrictedThreeBodyProblem )
{
    // Test 1: conversion of dimensionless to dimensional Cartesian state vector for test particle
    // in Sun-Jupiter system [m, m/s] (Koon, 2006).
    {
        namespace crtbp = gravitation::circular_restricted_three_body_problem;

        // Set distance between Sun and Jupiter [m] [NASA, 2010].
        double distanceSunJupiter = 7.784e11;

        // Set magnitude of velocity of Sun-Jupiter system [m/s].
        double velocitySunJupiter = 13.102e3;

        // Set gravitational parameter of Jupiter [m^3 s^-2].
        double gravitationalParameterJupiter = 6.67259e-11 * 1898.13e24;

        // Set gravitational parameter of Sun [m^3 s^-2].
        double gravitationalParameterSun = 1.32712440018e20;

        // Set mass parameter of Sun-Jupiter system.
        double massParameter = gravitationalParameterJupiter
                / ( gravitationalParameterJupiter + gravitationalParameterSun );

        // Set dimensionless state of the Sun.
        using orbital_element_conversions::xCartesianPositionIndex;
        Eigen::VectorXd dimensionlessStateOfSun = Eigen::VectorXd::Zero( 6 );
        dimensionlessStateOfSun( xCartesianPositionIndex ) = -massParameter;

        // Declare and set dimensionless state of Jupiter.
        Eigen::VectorXd dimensionlessStateOfJupiter = Eigen::VectorXd::Zero( 6 );
        dimensionlessStateOfJupiter( xCartesianPositionIndex ) = 1.0 - massParameter;

        // Set dimensionless state of test particle in Sun-Jupiter system.
        using orbital_element_conversions::yCartesianVelocityIndex;
        Eigen::VectorXd dimensionlessStateOfTestParticle = Eigen::VectorXd::Zero( 6 );
        dimensionlessStateOfTestParticle( xCartesianPositionIndex ) = 1.0;
        dimensionlessStateOfTestParticle( yCartesianVelocityIndex ) = 1.0;

        // Convert dimensionless to dimensional Cartesian state vector for test particle [m, m/s].
        Eigen::VectorXd computedDimensionalStateOfTestParticle( 6 );
        computedDimensionalStateOfTestParticle =
                crtbp::convertDimensionlessCartesianStateToDimensionalUnits(
                    dimensionlessStateOfTestParticle, gravitationalParameterSun,
                    gravitationalParameterJupiter, distanceSunJupiter );

        // Check if computed position and velocity matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( distanceSunJupiter,
                                    computedDimensionalStateOfTestParticle( xCartesianPositionIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( velocitySunJupiter,
                                    computedDimensionalStateOfTestParticle( yCartesianVelocityIndex ),
                                    1.0e-2 );
    }
}

//! Test if time conversion for CRTBP is computed correctly.
BOOST_AUTO_TEST_CASE( testTimeConversionCircularRestrictedThreeBodyProblem )
{
    namespace crtbp = gravitation::circular_restricted_three_body_problem;
    using mathematical_constants::PI;

    // Test 1: conversion of dimensionless to dimensional time for test particle in Sun-Jupiter
    // system [s] (Koon, 2006).
    {
        // Set distance between Sun and Jupiter [m] (NASA, 2010).
        double distanceSunJupiter = 7.784e11;

        // Set gravitational parameter of Jupiter [m^3 s^-2].
        double gravitationalParameterJupiter = 6.67259e-11 * 1898.13e24;

        // Set gravitational parameter of Sun [m^3 s^-2].
        double gravitationalParameterSun = 1.32712440018e20;

        // Compute dimensional time for one complete orbit.
        double computedDimensionalTime =
                crtbp::convertDimensionlessTimeToDimensionalTime(
                    2.0 * PI, gravitationalParameterSun, gravitationalParameterJupiter,
                    distanceSunJupiter );

        // Set expected orbital period of system [s].
        double expectedOrbitalPeriodOfSunJupiterSystem = 3.733e08;

        // Check if computed orbital period matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( expectedOrbitalPeriodOfSunJupiterSystem,
                                    computedDimensionalTime, 1.0e-2 );
    }
}

} // namespace unit_tests
} // namespace tudat


//==============================================================================
//         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/functions/repeat_upper_half.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>

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

NT2_TEST_CASE_TPL(repeat_upper_half, BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::native;
  using boost::simd::tag::repeat_upper_half_;
  using boost::simd::meta::cardinal_of;

  typedef BOOST_SIMD_DEFAULT_EXTENSION      ext_t;
  typedef native<T,ext_t>                      vT;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta
                                            ::call<repeat_upper_half_(vT)>::type
                  , vT
                  );

  const std::size_t card = cardinal_of<vT>::value;
  vT origin,ref;

  for(std::size_t i=0; i<card; ++i)
  {
    origin[i] = T(i+1);
    ref[i]    = i<(card/2) ? T(i+1+card/2): T(i+1);
  }

  NT2_TEST_EQUAL(boost::simd::repeat_upper_half(origin),ref);
}


#define BOOST_TEST_MODULE "test_global_pair_uniform_lennard_jones_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/NaivePairCalculation.hpp>
#include <mjolnir/forcefield/global/GlobalPairUniformLennardJonesInteraction.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(GlobalPairUniformLennardJonesInteraction_numeric_limits)
{
    mjolnir::LoggerManager::set_default_logger("test_global_pair_uniform_lennard_jones_interaction.log");
    using traits = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type        = traits::real_type;
    using coordinate_type  = traits::coordinate_type;
    using boundary_type    = traits::boundary_type;
    using system_type      = mjolnir::System<traits>;
    using topology_type    = mjolnir::Topology;
    using potential_type   = mjolnir::UniformLennardJonesPotential<traits>;
    using parameter_type   = typename potential_type::parameter_type;
    using partition_type   = mjolnir::NaivePairCalculation<traits, potential_type>;
    using interaction_type = mjolnir::GlobalPairInteraction<traits, potential_type>;

    auto normalize = [](const coordinate_type& v){return v / mjolnir::math::length(v);};

    potential_type   potential(1.0, 1.2, potential_type::default_cutoff(),
        std::vector<std::pair<std::size_t, parameter_type>>{
            {0, {}}, {1, {}}
        }, {}, typename potential_type::ignore_molecule_type("Nothing"),
               typename potential_type::ignore_group_type   ({})
        );

    interaction_type interaction(potential_type{potential},
        mjolnir::SpatialPartition<traits, potential_type>(
            mjolnir::make_unique<partition_type>()));

    system_type sys(2, boundary_type{});
    topology_type topol(2);

    sys.mass(0)  = 1.0;
    sys.mass(1)  = 1.0;
    sys.rmass(0) = 1.0;
    sys.rmass(1) = 1.0;

    sys.position(0) = coordinate_type(0.0, 0.0, 0.0);
    sys.position(1) = coordinate_type(0.5, 0.0, 0.0);
    sys.velocity(0) = coordinate_type(0.0, 0.0, 0.0);
    sys.velocity(1) = coordinate_type(0.0, 0.0, 0.0);
    sys.force(0)    = coordinate_type(0.0, 0.0, 0.0);
    sys.force(1)    = coordinate_type(0.0, 0.0, 0.0);

    topol.construct_molecules();

    sys.name(0)  = "X";
    sys.name(1)  = "X";
    sys.group(0) = "NONE";
    sys.group(1) = "NONE";

    interaction.initialize(sys, topol);

    std::mt19937 rng(123456789);
    std::normal_distribution<real_type> gauss(0.0, 1.0);

    const real_type rc = potential.max_cutoff_length();

    const real_type r_min = 0.5;
    const real_type r_max = rc;
    const real_type dr = 1e-3;

    const int max_count = (r_max - r_min) / dr;

    for(int i = 0; i < max_count-1; ++i)
    {
        const real_type dist = r_min + i * dr;

        sys.position(0) = coordinate_type(0,0,0);
        sys.position(1) = coordinate_type(0,0,0);
        sys.force(0)    = coordinate_type(0,0,0);
        sys.force(1)    = coordinate_type(0,0,0);

        sys.position(1) += dist *
            normalize(coordinate_type(gauss(rng), gauss(rng), gauss(rng)));

        const auto init = sys;

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;
        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE / dr == mjolnir::math::X(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE / dr == mjolnir::math::Y(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE / dr == mjolnir::math::Z(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(GlobalPairUniformLennardJonesInteraction_numeric_force_and_energy)
{
    mjolnir::LoggerManager::set_default_logger("test_global_pair_uniform_lennard_jones_interaction.log");
    using traits = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type        = traits::real_type;
    using coordinate_type  = traits::coordinate_type;
    using boundary_type    = traits::boundary_type;
    using system_type      = mjolnir::System<traits>;
    using topology_type    = mjolnir::Topology;
    using potential_type   = mjolnir::UniformLennardJonesPotential<traits>;
    using parameter_type   = typename potential_type::parameter_type;
    using partition_type   = mjolnir::NaivePairCalculation<traits, potential_type>;
    using interaction_type = mjolnir::GlobalPairInteraction<traits, potential_type>;

    auto normalize = [](const coordinate_type& v){return v / mjolnir::math::length(v);};

    potential_type   potential(1.0, 1.2, potential_type::default_cutoff(),
        std::vector<std::pair<std::size_t, parameter_type>>{
            {0, {}}, {1, {}}
        }, {}, typename potential_type::ignore_molecule_type("Nothing"),
               typename potential_type::ignore_group_type   ({})
        );

    interaction_type interaction(potential_type{potential},
        mjolnir::SpatialPartition<traits, potential_type>(
            mjolnir::make_unique<partition_type>()));

    system_type sys(2, boundary_type{});
    topology_type topol(2);

    sys.mass(0)  = 1.0;
    sys.mass(1)  = 1.0;
    sys.rmass(0) = 1.0;
    sys.rmass(1) = 1.0;

    sys.position(0) = coordinate_type(0.0, 0.0, 0.0);
    sys.position(1) = coordinate_type(0.5, 0.0, 0.0);
    sys.velocity(0) = coordinate_type(0.0, 0.0, 0.0);
    sys.velocity(1) = coordinate_type(0.0, 0.0, 0.0);
    sys.force(0)    = coordinate_type(0.0, 0.0, 0.0);
    sys.force(1)    = coordinate_type(0.0, 0.0, 0.0);

    topol.construct_molecules();

    sys.name(0)  = "X";
    sys.name(1)  = "X";
    sys.group(0) = "NONE";
    sys.group(1) = "NONE";

    interaction.initialize(sys, topol);

    std::mt19937 rng(123456789);
    std::normal_distribution<real_type> gauss(0.0, 1.0);

    const real_type rc = potential.max_cutoff_length();

    const real_type r_min = 0.5;
    const real_type r_max = rc;
    const real_type dr = 1e-3;

    const int max_count = (r_max - r_min) / dr;

    for(int i = 0; i < max_count-1; ++i)
    {
        const real_type dist = r_min + i * dr;

        sys.position(0) = coordinate_type(0,0,0);
        sys.position(1) = coordinate_type(0,0,0);
        sys.force(0)    = coordinate_type(0,0,0);
        sys.force(1)    = coordinate_type(0,0,0);

        sys.position(1) += dist *
            normalize(coordinate_type(gauss(rng), gauss(rng), gauss(rng)));

        system_type ref_sys = sys;

        constexpr real_type tol = 1e-4;

        const auto energy = interaction.calc_force_and_energy(sys);
        const auto ref_energy = interaction.calc_energy(ref_sys);
        interaction.calc_force(ref_sys);
        BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
        }
    }
}


#ifndef PARMCB_SVA_SIGNED_HPP_
#define PARMCB_SVA_SIGNED_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>
#include <boost/timer/timer.hpp>

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>

#include <parmcb/detail/signed_dijkstra.hpp>

#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/util.hpp>

namespace parmcb {

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_signed(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {

        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();

        /*
         * Initialize support vectors
         */
        std::vector<SpVecGF2<std::size_t>> support;
        for (std::size_t k = 0; k < csd; k++) {
            support.emplace_back(k);
        }

        boost::timer::cpu_timer cycle_timer;
        cycle_timer.stop();
        boost::timer::cpu_timer support_timer;
        support_timer.stop();

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        for (std::size_t k = 0; k < csd; k++) {
            /*
             * Choose the sparsest support heuristic
             */
            auto min_support = k;
            for (auto r = k + 1; r < csd; ++r) {
                if (support[r].size() < support[min_support].size())
                    min_support = r;
                if (support[min_support].size() < 5) {
                    break;
                }
            }
            if (min_support != k) {  // swap
                std::swap(support[k], support[min_support]);
            }

            /*
             * Compute shortest odd cycle
             */
            cycle_timer.resume();
            std::less<WeightType> compare = std::less<WeightType>();
            std::tuple<std::set<Edge>, WeightType, bool> best = std::make_tuple(std::set<Edge>(),
                    (std::numeric_limits<WeightType>::max)(), false);
            std::set<Edge> signed_edges;
            convert_edges(support[k], std::inserter(signed_edges, signed_edges.end()), forest_index);

            if (signed_edges.size() >= boost::num_vertices(g)) {
                VertexIt vi, viend;
                for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
                    auto v = *vi;
                    const bool use_hidden_edges = false;
                    auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges, std::set<Edge> { },
                            use_hidden_edges, v, true, v, false, std::get<2>(best), std::get<1>(best));
                    if (std::get<2>(res) && (!std::get<2>(best) || compare(std::get<1>(res), std::get<1>(best)))) {
                        best = res;
                        assert(std::get<2>(best));
                    }
                }
            } else {
                /*
                 * Heuristic in case number of signed edges is small compared to the number of vertices.
                 */
                std::set<Edge> hidden_edges;
                std::copy(signed_edges.begin(), signed_edges.end(), std::inserter(hidden_edges, hidden_edges.begin()));
                for (auto sei = signed_edges.begin(); sei != signed_edges.end(); ++sei) {
                    auto se = *sei;
                    auto se_v = boost::source(se, g);
                    auto se_u = boost::target(se, g);
                    auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges, hidden_edges, true, se_v,
                            true, se_u, true, std::get<2>(best), std::get<1>(best));
                    hidden_edges.erase(hidden_edges.begin());
                    if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                        std::get<1>(res) += boost::get(weight_map, se);
                        if (!std::get<2>(best) || compare(std::get<1>(res), std::get<1>(best))) {
                            std::get<0>(res).insert(se);
                            best = res;
                            assert(std::get<2>(best));
                        }
                    }
                }
            }
            assert(std::get<2>(best));
            cycle_timer.stop();

            /*
             * Update support vectors
             */
            support_timer.resume();
            std::set<std::size_t> cyclek;
            convert_edges(std::get<0>(best), std::inserter(cyclek, cyclek.end()), forest_index);
            for (std::size_t l = k + 1; l < csd; l++) {
                if (support[l] * cyclek == 1) {
                    support[l] += support[k];
                }
            }
            support_timer.stop();

            /*
             * Output new cycle
             */
            std::list<Edge> cyclek_edgelist;
            std::copy(std::get<0>(best).begin(), std::get<0>(best).end(), std::back_inserter(cyclek_edgelist));
            *out++ = cyclek_edgelist;
            mcb_weight += std::get<1>(best);
        }

        std::cout << "cycle   timer" << cycle_timer.format();
        std::cout << "support timer" << support_timer.format();

        return mcb_weight;
    }

} // parmcb

#endif