// 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)

// Copyright Paul A. Bristow 2015 - 2016.
// Copyright Christopher Kormanyos 2015 - 2016.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

// Below are snippets of code that can be included into a Quickbook file.

#include <iostream>
#include <iomanip>
#include <exception>
#include <typeinfo>

#include <boost/fixed_point/fixed_point.hpp>

//[fixed_point_type_representation_example_3

template <typename T>
void show_representation_types(std::ostream& os = std::cout)
{
  os << "Fixed_point Type "
    << typeid(T).name()
    // class boost::fixed_point::negatable<7,-1,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined>
    << " with range " << T::range << ", resolution " << T::resolution << std::endl;

  os << "value_type is " << typeid(T::value_type).name() << std::endl; // signed char
  os << "float_type is " << typeid(T::float_type).name() << std::endl; // float
} // template <typename T>void show_representation_types()

//] [/fixed_point_type_representation_example_3]

int main()
{
  using boost::fixed_point::negatable;

  typedef negatable<7, -1> fixed_point_type_7m1;
  typedef negatable<15, -16> fixed_point_type_15m16;
  typedef negatable<0, -15> fixed_point_type_0m15;
  typedef negatable<0, -31> fixed_point_type_0m31;
  typedef negatable<10, -21> fixed_point_type_10m21;
  typedef negatable<0, -63> fixed_point_type_0m63;
  typedef negatable<10, -53> fixed_point_type_10m53;
  typedef negatable<2, -78> fixed_point_type_2m78;
  typedef negatable<0, -126> fixed_point_type_0m126; //
  typedef negatable<10, -116> fixed_point_type_10m116; //
  typedef negatable<0, -254> fixed_point_type_0m254;

  try
  {
    std::cout.setf(std::ios_base::boolalpha | std::ios_base::showpoint); // Show any trailing zeros.
    std::cout << std::endl;

//[fixed_point_type_representation_example_1
     std::cout << typeid(negatable<0, -7>::value_type).name() << std::endl;
//] [/fixed_point_type_representation_example_1]

//[fixed_point_type_representation_example_2
     std::cout << typeid(negatable<0, -7>::float_type).name() << std::endl;
//] [/fixed_point_type_representation_example_2]

   show_representation_types<fixed_point_type_7m1>();
   show_representation_types<fixed_point_type_15m16>();
   show_representation_types<fixed_point_type_0m15>();
   show_representation_types<fixed_point_type_0m31>();
   show_representation_types<fixed_point_type_10m21>();
   show_representation_types<fixed_point_type_0m63>();
   show_representation_types<fixed_point_type_10m53>();
   show_representation_types<fixed_point_type_2m78>();
   show_representation_types<fixed_point_type_0m126>();
   show_representation_types<fixed_point_type_10m116>();
   show_representation_types<fixed_point_type_0m254>();

  }
  catch (std::exception ex)
  {
    std::cout << ex.what() << std::endl;
  }
}

/*
//[fixed_point_representation_examples_output_1

GCC5.1.0

signed char
float
Fixed_point Type class boost::fixed_point::negatable<7,-1,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 7, resolution -1
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 15, resolution -16
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-15,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -15
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<0,-31,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -31
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<10,-21,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -21
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-63,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -63
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-53,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -53
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<2,-78,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 2, resolution -78
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<80,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-126,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -126
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-116,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -116
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-254,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -254
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<256,256,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<254,2,void,int,0,0>,0>

//] [/fixed_point_representation_examples_output_1]

//[fixed_point_representation_examples_output_2

VS 2013

Fixed_point Type class boost::fixed_point::negatable<7,-1,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 7, resolution -1
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 15, resolution -16
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-15,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -15
value_type is short
float_type is float
Fixed_point Type class boost::fixed_point::negatable<0,-31,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -31
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<10,-21,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -21
value_type is int
float_type is double
Fixed_point Type class boost::fixed_point::negatable<0,-63,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -63
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-53,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -53
value_type is __int64
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<63,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<2,-78,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 2, resolution -78
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<80,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-126,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -126
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<10,-116,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 10, resolution -116
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<128,128,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<126,2,void,int,0,0>,0>
Fixed_point Type class boost::fixed_point::negatable<0,-254,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined> with range 0, resolution -254
value_type is class boost::multiprecision::number<struct boost::multiprecision::backends::cpp_int_backend<256,256,1,0,void>,0>
float_type is class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<254,2,void,int,0,0>,0>

//] [/fixed_point_representation_examples_output_2]

*/


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    smallExample.cpp
 * @brief   Create small example with two poses and one landmark
 * @brief   smallExample
 * @author  Carlos Nieto
 * @author  Frank dellaert
 */

#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/Matrix.h>

#include <tests/smallExample.h>

#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>

#include <iostream>
#include <string>

using namespace std;

namespace gtsam {
namespace example {

	using namespace gtsam::noiseModel;

	typedef boost::shared_ptr<NonlinearFactor> shared;

	static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
	static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
	static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
	static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);

	static const Index _l1_=0, _x1_=1, _x2_=2;
	static const Index _x_=0, _y_=1, _z_=2;

	// Convenience for named keys
	using symbol_shorthand::X;
	using symbol_shorthand::L;

	/* ************************************************************************* */
	boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
		// Create
		boost::shared_ptr<Graph> nlfg(
				new Graph);

		// prior on x1
		Point2 mu;
		shared f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
		nlfg->push_back(f1);

		// odometry between x1 and x2
		Point2 z2(1.5, 0);
		shared f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
		nlfg->push_back(f2);

		// measurement between x1 and l1
		Point2 z3(0, -1);
		shared f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
		nlfg->push_back(f3);

		// measurement between x2 and l1
		Point2 z4(-1.5, -1.);
		shared f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
		nlfg->push_back(f4);

		return nlfg;
	}

	/* ************************************************************************* */
	Graph createNonlinearFactorGraph() {
		return *sharedNonlinearFactorGraph();
	}

	/* ************************************************************************* */
	Values createValues() {
		Values c;
		c.insert(X(1), Point2(0.0, 0.0));
		c.insert(X(2), Point2(1.5, 0.0));
		c.insert(L(1), Point2(0.0, -1.0));
		return c;
	}

	/* ************************************************************************* */
	VectorValues createVectorValues() {
		VectorValues c(vector<size_t>(3, 2));
		c[_l1_] = Vector_(2, 0.0, -1.0);
		c[_x1_] = Vector_(2, 0.0, 0.0);
		c[_x2_] = Vector_(2, 1.5, 0.0);
		return c;
	}

	/* ************************************************************************* */
	boost::shared_ptr<const Values> sharedNoisyValues() {
		boost::shared_ptr<Values> c(new Values);
		c->insert(X(1), Point2(0.1, 0.1));
		c->insert(X(2), Point2(1.4, 0.2));
		c->insert(L(1), Point2(0.1, -1.1));
		return c;
	}

	/* ************************************************************************* */
	Values createNoisyValues() {
		return *sharedNoisyValues();
	}

	/* ************************************************************************* */
	VectorValues createCorrectDelta(const Ordering& ordering) {
		VectorValues c(vector<size_t>(3,2));
		c[ordering[L(1)]] = Vector_(2, -0.1, 0.1);
		c[ordering[X(1)]] = Vector_(2, -0.1, -0.1);
		c[ordering[X(2)]] = Vector_(2, 0.1, -0.2);
		return c;
	}

	/* ************************************************************************* */
	VectorValues createZeroDelta(const Ordering& ordering) {
		VectorValues c(vector<size_t>(3,2));
		c[ordering[L(1)]] = zero(2);
		c[ordering[X(1)]] = zero(2);
		c[ordering[X(2)]] = zero(2);
		return c;
	}

	/* ************************************************************************* */
	GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
		// Create empty graph
	  GaussianFactorGraph fg;

		SharedDiagonal unit2 = noiseModel::Unit::Create(2);

		// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2));

		// odometry between x1 and x2: x2-x1=[0.2;-0.1]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2));

    // measurement between x1 and l1: l1-x1=[0.0;0.2]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2));

		// measurement between x2 and l1: l1-x2=[-0.2;0.3]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));

		return fg;
	}

	/* ************************************************************************* */
	/** create small Chordal Bayes Net x <- y
	 * x y d
	 * 1 1 9
	 *   1 5
	 */
	GaussianBayesNet createSmallGaussianBayesNet() {
		Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
		Matrix R22 = Matrix_(1, 1, 1.0);
		Vector d1(1), d2(1);
		d1(0) = 9;
		d2(0) = 5;
		Vector tau(1);
		tau(0) = 1.0;

		// define nodes and specify in reverse topological sort (i.e. parents last)
		GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau));
		GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau));
		GaussianBayesNet cbn;
		cbn.push_back(Px_y);
		cbn.push_back(Py);

		return cbn;
	}

	/* ************************************************************************* */
	// Some nonlinear functions to optimize
	/* ************************************************************************* */
	namespace smallOptimize {

		Point2 h(const Point2& v) {
			return Point2(cos(v.x()), sin(v.y()));
		}

		Matrix H(const Point2& v) {
			return Matrix_(2, 2,
					-sin(v.x()), 0.0,
					 0.0, cos(v.y()));
		}

		struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {

			Point2 z_;

			UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
				gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) {
			}

			Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
				if (A) *A = H(x);
				return (h(x) - z_).vector();
			}

		};

	}

	/* ************************************************************************* */
	boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
		boost::shared_ptr<Graph> fg(new Graph);
		Vector z = Vector_(2, 1.0, 0.0);
		double sigma = 0.1;
		boost::shared_ptr<smallOptimize::UnaryFactor> factor(
				new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
		fg->push_back(factor);
		return fg;
	}

	Graph createReallyNonlinearFactorGraph() {
		return *sharedReallyNonlinearFactorGraph();
	}

	/* ************************************************************************* */
	pair<Graph, Values> createNonlinearSmoother(int T) {

		// Create
		Graph nlfg;
		Values poses;

		// prior on x1
		Point2 x1(1.0, 0.0);
		shared prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
		nlfg.push_back(prior);
		poses.insert(X(1), x1);

		for (int t = 2; t <= T; t++) {
			// odometry between x_t and x_{t-1}
			Point2 odo(1.0, 0.0);
			shared odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
			nlfg.push_back(odometry);

			// measurement on x_t is like perfect GPS
			Point2 xt(t, 0);
			shared measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
			nlfg.push_back(measurement);

			// initial estimate
			poses.insert(X(t), xt);
		}

		return make_pair(nlfg, poses);
	}

	/* ************************************************************************* */
	pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
		Graph nlfg;
		Values poses;
		boost::tie(nlfg, poses) = createNonlinearSmoother(T);

		if(!ordering) ordering = *poses.orderingArbitrary();
		return make_pair(*nlfg.linearize(poses, *ordering), *ordering);
	}

	/* ************************************************************************* */
	GaussianFactorGraph createSimpleConstraintGraph() {
		// create unary factor
		// prior on _x_, mean = [1,-1], sigma=0.1
		Matrix Ax = eye(2);
		Vector b1(2);
		b1(0) = 1.0;
		b1(1) = -1.0;
		JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));

		// create binary constraint factor
		// between _x_ and _y_, that is going to be the only factor on _y_
		// |1 0||x_1| + |-1  0||y_1| = |0|
		// |0 1||x_2|   | 0 -1||y_2|   |0|
		Matrix Ax1 = eye(2);
		Matrix Ay1 = eye(2) * -1;
		Vector b2 = Vector_(2, 0.0, 0.0);
		JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
				constraintModel));

		// construct the graph
		GaussianFactorGraph fg;
		fg.push_back(f1);
		fg.push_back(f2);

		return fg;
	}

	/* ************************************************************************* */
	VectorValues createSimpleConstraintValues() {
		VectorValues config(vector<size_t>(2,2));
		Vector v = Vector_(2, 1.0, -1.0);
		config[_x_] = v;
		config[_y_] = v;
		return config;
	}

	/* ************************************************************************* */
	GaussianFactorGraph createSingleConstraintGraph() {
		// create unary factor
		// prior on _x_, mean = [1,-1], sigma=0.1
		Matrix Ax = eye(2);
		Vector b1(2);
		b1(0) = 1.0;
		b1(1) = -1.0;
		//GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
		JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));

		// create binary constraint factor
		// between _x_ and _y_, that is going to be the only factor on _y_
		// |1 2||x_1| + |10 0||y_1| = |1|
		// |2 1||x_2|   |0 10||y_2|   |2|
		Matrix Ax1(2, 2);
		Ax1(0, 0) = 1.0;
		Ax1(0, 1) = 2.0;
		Ax1(1, 0) = 2.0;
		Ax1(1, 1) = 1.0;
		Matrix Ay1 = eye(2) * 10;
		Vector b2 = Vector_(2, 1.0, 2.0);
		JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
				constraintModel));

		// construct the graph
		GaussianFactorGraph fg;
		fg.push_back(f1);
		fg.push_back(f2);

		return fg;
	}

	/* ************************************************************************* */
	VectorValues createSingleConstraintValues() {
		VectorValues config(vector<size_t>(2,2));
		config[_x_] = Vector_(2, 1.0, -1.0);
		config[_y_] = Vector_(2, 0.2, 0.1);
		return config;
	}

	/* ************************************************************************* */
	GaussianFactorGraph createMultiConstraintGraph() {
		// unary factor 1
		Matrix A = eye(2);
		Vector b = Vector_(2, -2.0, 2.0);
		JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));

		// constraint 1
		Matrix A11(2, 2);
		A11(0, 0) = 1.0;
		A11(0, 1) = 2.0;
		A11(1, 0) = 2.0;
		A11(1, 1) = 1.0;

		Matrix A12(2, 2);
		A12(0, 0) = 10.0;
		A12(0, 1) = 0.0;
		A12(1, 0) = 0.0;
		A12(1, 1) = 10.0;

		Vector b1(2);
		b1(0) = 1.0;
		b1(1) = 2.0;
		JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
				constraintModel));

		// constraint 2
		Matrix A21(2, 2);
		A21(0, 0) = 3.0;
		A21(0, 1) = 4.0;
		A21(1, 0) = -1.0;
		A21(1, 1) = -2.0;

		Matrix A22(2, 2);
		A22(0, 0) = 1.0;
		A22(0, 1) = 1.0;
		A22(1, 0) = 1.0;
		A22(1, 1) = 2.0;

		Vector b2(2);
		b2(0) = 3.0;
		b2(1) = 4.0;
		JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
				constraintModel));

		// construct the graph
		GaussianFactorGraph fg;
		fg.push_back(lf1);
		fg.push_back(lc1);
		fg.push_back(lc2);

		return fg;
	}

	/* ************************************************************************* */
	VectorValues createMultiConstraintValues() {
		VectorValues config(vector<size_t>(3,2));
		config[_x_] = Vector_(2, -2.0, 2.0);
		config[_y_] = Vector_(2, -0.1, 0.4);
		config[_z_] = Vector_(2, -4.0, 5.0);
		return config;
	}

	/* ************************************************************************* */
	// Create key for simulated planar graph
	Symbol key(int x, int y) {
		return X(1000*x+y);
	}

	/* ************************************************************************* */
	boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {

		// create empty graph
		NonlinearFactorGraph nlfg;

		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), Isotropic::Sigma(2,1e-3), key(1,1)));
		nlfg.push_back(constraint);

		// Create horizontal constraints, 1...N*(N-1)
		Point2 z1(1.0, 0.0); // move right
		for (size_t x = 1; x < N; x++)
			for (size_t y = 1; y <= N; y++) {
				shared f(new simulated2D::Odometry(z1, Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y)));
				nlfg.push_back(f);
			}

		// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
		Point2 z2(0.0, 1.0); // move up
		for (size_t x = 1; x <= N; x++)
			for (size_t y = 1; y < N; y++) {
				shared f(new simulated2D::Odometry(z2, Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1)));
				nlfg.push_back(f);
			}

		// Create linearization and ground xtrue config
		Values zeros;
    for (size_t x = 1; x <= N; x++)
      for (size_t y = 1; y <= N; y++)
        zeros.insert(key(x, y), Point2());
		Ordering ordering(planarOrdering(N));
		VectorValues xtrue(zeros.dims(ordering));
		for (size_t x = 1; x <= N; x++)
			for (size_t y = 1; y <= N; y++)
				xtrue[ordering[key(x, y)]] = Point2(x,y).vector();

		// linearize around zero
		boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
		return boost::make_tuple(*gfg, xtrue);
	}

	/* ************************************************************************* */
	Ordering planarOrdering(size_t N) {
		Ordering ordering;
		for (size_t y = N; y >= 1; y--)
			for (size_t x = N; x >= 1; x--)
				ordering.push_back(key(x, y));
		return ordering;
	}

	/* ************************************************************************* */
	pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
			const GaussianFactorGraph& original) {
		GaussianFactorGraph T, C;

		// Add the x11 constraint to the tree
		T.push_back(original[0]);

		// Add all horizontal constraints to the tree
		size_t i = 1;
		for (size_t x = 1; x < N; x++)
			for (size_t y = 1; y <= N; y++, i++)
				T.push_back(original[i]);

		// Add first vertical column of constraints to T, others to C
		for (size_t x = 1; x <= N; x++)
			for (size_t y = 1; y < N; y++, i++)
				if (x == 1)
					T.push_back(original[i]);
				else
					C.push_back(original[i]);

		return make_pair(T, C);
	}

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

} // example
} // namespace gtsam


#pragma once

#include <vector>

#include <Eigen/Geometry>
#include <iostream>
#include <geometry_msgs/Vector3.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_model/robot_model.h>

#include "../include/util/typedefs.hpp"

class OmniMath {
    private:

    MovingAverage moving_average(5,6);

    const bool enable_moving_average;

    public:

    template <typename Type>
    Type saturate(Type value, Type min, Type max) {
        if (value < min) {
            return min;
        }
        if (value > max) {
            return max;
        }
        return value;
    }

    template <typename Type>
    std::vector<Type> saturate(std::vector<Type> values, Type min, Type max) {
        std::vector<Type> s(values.size())
        for (auto iter = values.begin(); iter < values.end(); ++iter) {
            s[iter-values.begin()] = saturate(*iter, min, max);
        }
        return s;
    }

    Eigen::Affine3d fowardKinematics(
        robot_state::RobotStatePtr kinematic_state,
        std::string link_name) {
            return kinematic_state->getGlobalLinkTransform(link_name);
    }

    Eigen::Vector3d OmniBase::calculateTorqueFeedback(
        robot_state::RobotStatePtr kinematic_state,
        robot_state::JointModelGroup* joint_model_group,
        //const Eigen::Vector3d& force,
        const Eigen::Vector3d force,
        Eigen::Matrix3d rot_link_to_teleop,
        double feedback_gain,
        std::string end_effector_name,
        const robot_state::LinkModel* end_effector_link_model) {
            if (feedback_gain == 0)
                return Eigen::Vector3d::Zero();

            // Get the Jacobian matrix written on the base frame
            Eigen::Vector3d origin(0,0.08,0);
            Eigen::MatrixXd jacobian;
            auto link_model = kinematic_state->getLinkModel(end_effector_name);
            kinematic_state->getJacobian(joint_model_group, link_model, origin, jacobian, false);

            // Rotate the force vector from the desired frame to the base frame
            Eigen::Vector3d force_on_link_frame = rot_link_to_teleop * force;
            //
            auto quat_base_link = fowardKinematics(kinematic_state, end_effector_name)
            force_on_link_frame = quat_base_link.conjugate() * force_on_link_frame;

            auto ret = feedback_gain * jacobian.block<3,3>(0,0).transpose() * force_on_link_frame;
            return ret.block<3,1>(0,0);
        }

    Eigen::VectorXd jerkSaturationFilter(
        Eigen::VectorXd previous_calculated_velocities,
        Eigen::VectorXd current_calculated_velocities,
        double max_jerk) {
            std:vector<double> saturated_velocities(6);
            for (int i = 0; i < 6; ++i) {
                if ( std::abs(current_calculated_velocities[i] - previous_calculated_velocites[i]) < max_jerk ) {
                    saturated_velocities[i] = current_calculated_velocities[i];
                }
                else {
                    saturated_velocities[i] = previous_calculated_velocities[i];
                }
            }
            Eigen::VectorXd eigen_vector(saturated_velocities.data());
            return eigen_vector;
        }

    Eigen::VectorXd calculateVelocities(
        Time previous_measurement_time,
        Time current_measurement_time,
        Eigen::VectorXd previous_measurement,
        Eigen::VecotrXd current_measurement,
        bool enable_moving_average) {
            double delta_angle;
            double delta_t = (current_measurement_time - previous_measurement_time).total_microseconds();
            std::vector<double> calculated_velocities(6);
            for (int i = 0; i < 6; ++i) {
                deltaAngle = current_measurement[i] - previous_measurement[i];
                calculated_velocities[i] = deltaAngle * 1000000 / dt;
            }
            if (enable_moving_average) {
                moving_average.input(calculated_velocities);
                Eigen::VectorXd joint_velocities(moving_average.mean.data());
            }
            else Eigen::VectorXd joint_velocities(calculated_velocities.data());
            return joint_velocities(6);
        }

    Eigen::VectorXd calculateTwist(
        robot_state::RobotStatePtr kinematic_state,
        robot_state::JointModelGroup* joint_model_group,
        robot_state::LinkModel* end_effector_link_model,
        Eigen::VectorXd current_joint_velocities) {
        std::vector<double> twist;
        Eigen::Vector3d origin(0,0,0);
        Eigen::MatrixXd jacobian(6,6);
        kinematic_state->getJacobian(joint_model_group, end_effector_link_model, origin, jacobian, false);
        return jacobian * current_joint_velocities;
    }

public:
    /**
     * @brief OmniBase constructor, sets some members and prepares ros topics and publishers.
     * @param name Reference to string of omni name.
     */
    explicit OmniMath(const bool enable_moving_average);
};


#include <math.h>
#include <cmath>
#include <RcppEigen.h>
#include <Rcpp.h>
#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <iostream>
// [[Rcpp::depends(RcppEigen)]]

typedef Eigen::Triplet<float> T; //tripleta para rellenar matrices sparse
typedef Eigen::MappedSparseMatrix<float> MSpMat; //alias para matriz sparse
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd; //alias para vector sparse
using namespace Rcpp;

// [[Rcpp::export]]
Eigen::SparseMatrix<float> Kernel_float ( NumericMatrix M, int h, int w, double distancia, double sigJ, double sigd)
{
  // Funcion para calcular  la matriz de similaridades de todos los puntos de una imagen
  //  input: w: altura en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         h: ancho en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         distancia: limite de la vecindad del pixel a considerar
  //         M: imagen de 2 dimensiones (EN ESCALA DE GRISES)
  //         sigJ: sigma para kernel gaussiano, de la imagen
  //         sigd: sigma para kernell gaussiano, de las distancias
  //output: W, similaridad calculada, sparse
  int i, j, k, l = 0 ;
  float vecindad = (float)distancia;
  float d2 = 0.;
  float calculo = 0.;
  std::vector<T> tripletList; //inicializamos tripleta
  tripletList.reserve((int)floor(h*h*w*w*.1));
  //comienza calculo de matriz de similaridades
  for(i=0; i < h; i++)
  {
    for(j=0; j< w; j++ )
    {
      for(k=0; k<h; k++)
      {
        for(l=0; l<w; l++)
        {
          d2 = (float)(pow(i-k,2)+pow(j-l,2));
          if(d2 <= vecindad)
          {
            calculo = (float) (exp(-pow(M(j,i)-M(l,k),2)/(2*pow(sigJ,2))-d2/(2*pow(sigd,2))));
            tripletList.push_back(T((j)*h+i, (l)*h+k, calculo ));
          }
        }
      }
    }
  }
  //termina calculo de matriz de similaridades
  Eigen::SparseMatrix<float>  W(h*w, h*w); //construccion de matriz de similaridades
  W.setFromTriplets(tripletList.begin(), tripletList.end());
  return(W);
}


// Std includes
#include <cmath>
#include <iostream>
#include <memory>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point.h"
#include "sl0/group/dynamic.h"
// Simple includes
#include "flow.h"

using TypeScalar = double;
// State
template<int Size>
using TypeVector = Eigen::Matrix<TypeScalar, Size, 1>;
// Space
constexpr unsigned int DIM = 3;
using TypeSpaceVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Group Parameters
using TypeStepPoint = sl0::StepPoint<TypeVector, DIM, TypeView, Flow>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg<TypeVector<Eigen::Dynamic>, TypeView>;

int main () {
    // Parameters
    TypeSpaceVector x0 = TypeSpaceVector::Constant(1.0);
    TypeScalar t0 = 0.0;
    TypeScalar dt = 1e-3;
    unsigned int nt = std::round(1.0 / dt);
    unsigned int np = 1000;
    // Create group
    sl0::GroupDynamic<TypeVector, DIM, TypeView, TypeSolver> group;
    // Ceate points
    std::cout << "Point creation" << "\n";
    std::shared_ptr<TypeStepPoint> sStepPoint = std::make_shared<TypeStepPoint>(std::make_shared<Flow>());
    for(std::size_t i = 0; i < np; i++) {
        group.sStep->addMember(group.state, std::make_shared<TypeStepPoint>(*sStepPoint), DIM);
        sStepPoint->x(group.sStep->memberState(group.state.data(), i)) = x0;
    }
    // Set initial state
    std::cout << "Point init" << "\n";
    for(std::size_t n = 0; n < group.sStep->size(); n++) {
        sStepPoint->x(group.sStep->memberState(group.state.data(), n)) = x0;
    }
    group.t = t0;
    // Computation
    std::cout << "Computing" << "\n";
    for(std::size_t i = 0; i < nt; i++) {
        group.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "Group advected following a an exponential flow, exp(" << group.t << ") = " << "\n";
    std::cout << "\n";
    for(std::size_t n = 0; n < group.sStep->size(); n++) {
        std::cout << "Group Final Position : " << "\n" << sStepPoint->x(group.sStep->memberState(group.state.data(), n)) << "\n";
    }
    std::cout << std::endl;
}


#include "LinearCodeGenerator.h"
#include "Math.h"

#include <unordered_map>
#include <gurobi_c++.h>
#include <armadillo>
#include <random>

/* Configuration */
constexpr bool SHOW_GUROBI_OUTPUT = true;
constexpr bool SHOW_MATRIX_OUTPUT = false;
constexpr uint32_t TIMEOUT_MATRIX_REDUCE = 1;

LinearCodeGenerator::LinearCodeGenerator(uint16_t q, uint16_t k, uint16_t b)
	: m_q(q)
	, m_k(k)
	, m_b(b)
{

}

auto LinearCodeGenerator::generateCode() const -> LinearCode
{
	std::cout << "Generating canonical represents" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;

	auto canonialRepresents = Math::generateCanonialRepresents(m_q, m_k);
	if (SHOW_MATRIX_OUTPUT)
	{
		for (arma::uword i = 0; i < canonialRepresents.size(); ++i)
		{
			std::cout << "r" << i << "\t";
			canonialRepresents.at(i).t().raw_print();
		}
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Generating matrix A" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto matrixA = generateMatrixA(canonialRepresents);
	if (SHOW_MATRIX_OUTPUT)
	{
		matrixA.raw_print();
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Generating producer e0" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto producerE0 = generateProducerE0(canonialRepresents, matrixA);

	const auto colCycles = cycleCheck(canonialRepresents, producerE0, m_q);
	const auto rowCycles = cycleCheck(canonialRepresents, producerE0.t(), m_q);

	auto colGroups = generateRepresentGroups(colCycles, canonialRepresents.size());
	auto rowGroups = generateRepresentGroups(rowCycles, canonialRepresents.size());

	std::cout << "Found Producer e0 - reducing to [" << colGroups.size() << "] groups" << std::endl;
	if (SHOW_MATRIX_OUTPUT)
	{
		producerE0.raw_print();
		std::cout << "----------------------------------------------------" << std::endl << std::endl;
		std::cout << "Found cycles for columns" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl;
		for (auto& group : colGroups)
		{
			std::cout << "S([r" << group.at(0) << "]) = {";
			for (arma::uword i = 0; i < group.size(); i++)
			{
				std::cout << "[r" << group.at(i) << "]";
				if (i != group.size() - 1) std::cout << ", ";
			}
			std::cout << "}" << std::endl;
		}

		std::cout << std::endl;
		std::cout << "Found cycles for rows" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl;
		for (auto& group : rowGroups)
		{
			std::cout << "ST([r" << group.at(0) << "]) = {";
			for (arma::uword i = 0; i < group.size(); i++)
			{
				std::cout << "[r" << group.at(i) << "]";
				if (i != group.size() - 1) std::cout << ", ";
			}
			std::cout << "}" << std::endl;
		}
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Re-calculate matrix A with cylces" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto optMatrixA = generateOptimizedMatrixA(matrixA, colCycles, rowCycles);
	if (SHOW_MATRIX_OUTPUT)
	{
		optMatrixA.raw_print();
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Calculate vector c" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto vectorC = generateVectorC(colGroups);
	vectorC.t().raw_print();
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Calculating vector x" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	auto vectorX = generateVectorX(optMatrixA, vectorC, m_b);
	vectorX.t().raw_print();
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	const uint16_t n = dot(vectorX, vectorC);
	const uint16_t d = n - m_b;
	auto G = generateMatrixG(vectorX, colGroups, canonialRepresents);

	return { n, m_q, m_k, d, G };
}

auto LinearCodeGenerator::generateProducerE0(const std::vector<arma::s32_vec>& canonialRepresents, const arma::s32_mat& matrixA) const -> arma::s32_mat
{
	arma::s32_mat e0(m_k, m_k);
	arma::s32_mat test_e0(m_k, m_k);
	test_e0.fill(0);
	auto colGroupCount = std::numeric_limits<uint32_t>::max();
	uint32_t timeout = 0;

	std::chrono::high_resolution_clock::time_point start;
	do
	{
		auto colCycles = cycleCheck(canonialRepresents, test_e0, m_q);
		auto colGroups = generateRepresentGroups(colCycles, canonialRepresents.size());

		uint32_t maxElements = 0;
		for (auto& colGroup : colGroups)
		{
			if (colGroup.size() > maxElements)
			{
				maxElements = colGroup.size();
			}
		}
		if (maxElements < m_q && colGroups.size() < colGroupCount)
		{
			colGroupCount = colGroups.size();
			e0 = test_e0;
			timeout = 0;
		}

		const auto end = std::chrono::high_resolution_clock::now();
		const auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
		if (duration > 1000)
		{
			std::cout << "Current producer e0 reduces A from [" << matrixA.n_cols << "] to [" << colGroupCount << "] columns." << std::endl;
			start = end;
			++timeout;
		}

		test_e0 = generateMatrixE0();
	} while (colGroupCount == matrixA.n_cols || timeout < TIMEOUT_MATRIX_REDUCE);

	return e0;
}

auto LinearCodeGenerator::generateOptimizedMatrixA(const arma::s32_mat& matrixA,
	const std::unordered_map<arma::uword, std::vector<arma::uword>>& colCycles,
	const std::unordered_map<arma::uword, std::vector<arma::uword>>& rowCycles) const -> arma::s32_mat
{
	auto optMatrixA = matrixA;
	std::vector<arma::uword> colsDelete;
	std::vector<arma::uword> rowsDelete;

	for (auto& c : colCycles)
	{
		for (arma::uword i = 1; i < c.second.size(); ++i)
		{
			for (arma::uword j = 0; j < optMatrixA.n_rows; j++)
			{
				optMatrixA(j, c.second.at(0)) += optMatrixA(j, c.second.at(i));
			}

			colsDelete.push_back(c.second.at(i));
		}
	}

	for (auto& r : rowCycles)
	{
		for (arma::uword i = 1; i < r.second.size(); ++i)
		{
			rowsDelete.push_back(r.second.at(i));
		}
	}

	sort(colsDelete.begin(), colsDelete.end());
	reverse(colsDelete.begin(), colsDelete.end());
	for (auto& c : colsDelete)
	{
		optMatrixA.shed_col(c);
	}

	sort(rowsDelete.begin(), rowsDelete.end());
	reverse(rowsDelete.begin(), rowsDelete.end());
	for (auto& r : rowsDelete)
	{
		optMatrixA.shed_row(r);
	}

	return optMatrixA;
}

auto LinearCodeGenerator::generateMatrixG(const arma::s32_vec& xVec, const std::vector<std::vector<arma::uword>>& colGroups,
	const std::vector<arma::s32_vec>& rVector) -> arma::s32_mat
{
	uint32_t colSize = 0;
	for (uint32_t i = 0; i < xVec.size(); ++i)
	{
		colSize += xVec.at(i) * colGroups.at(i).size();
	}

	const auto rowSize = rVector.at(0).size();
	arma::s32_mat G(rowSize, colSize, arma::fill::zeros);

	int colIndex = 0;
	for (uint32_t x = 0; x < xVec.size(); ++x)
	{
		for (auto i = 0; i < xVec.at(x); ++i)
		{
			for (unsigned int j : colGroups.at(x))
			{
				for (uint32_t k = 0; k < rowSize; ++k)
				{
					G(k, colIndex) = rVector.at(j).at(k);
				}
				colIndex++;
			}
		}
	}

	return G;
}

auto LinearCodeGenerator::generateVectorC(const std::vector<std::vector<arma::uword>>& colGroups) -> arma::s32_vec
{
	arma::s32_vec vectorC(colGroups.size());

	for (arma::uword i = 0; i < colGroups.size(); ++i)
	{
		vectorC.at(i) = colGroups.at(i).size();
	}

	return vectorC;
}

auto LinearCodeGenerator::generateVectorX(const arma::s32_mat& matrixA, const arma::s32_vec& vectorC, uint16_t b) -> arma::s32_vec
{
	const auto env = GRBEnv();
	auto model = GRBModel(env);
	model.getEnv().set(GRB_IntParam_OutputFlag, SHOW_GUROBI_OUTPUT);

	std::vector<GRBVar> modelVars;
	modelVars.reserve(vectorC.size());
	for (uint32_t i = 0; i < vectorC.size(); i++)
	{
		modelVars.push_back(model.addVar(0.0, GRB_INFINITY, 0.0, GRB_INTEGER));
	}

	GRBLinExpr expr = 0.0;
	for (uint32_t i = 0; i < vectorC.size(); i++)
	{
		expr += modelVars.at(i) * vectorC.at(i);
	}

	model.setObjective(expr, GRB_MAXIMIZE);

	for (arma::uword x = 0; x < matrixA.n_rows; x++)
	{
		expr = 0.0;
		for (arma::uword y = 0; y < matrixA.n_cols; y++)
		{
			expr += modelVars.at(y) * matrixA(x, y);
		}
		model.addConstr(expr, GRB_LESS_EQUAL, b);
	}

	//model.update();
	//model.write("debug.lp");
	model.optimize();

	arma::s32_vec vectorX(modelVars.size());
	for (uint32_t i = 0; i < modelVars.size(); ++i)
	{
		vectorX.at(i) = static_cast<int>(modelVars.at(i).get(GRB_DoubleAttr_X));
	}

	return vectorX;
}

auto LinearCodeGenerator::generateMatrixA(const std::vector<arma::s32_vec>& canonialRepresents) const -> arma::s32_mat
{
	auto matrixA = arma::s32_mat(canonialRepresents.size(), canonialRepresents.size(), arma::fill::zeros);

	for (arma::uword row = 0; row < canonialRepresents.size(); ++row)
	{
		for (arma::uword col = 0; col < canonialRepresents.size(); ++col)
		{
			if (static_cast<int>(dot(canonialRepresents.at(row), canonialRepresents.at(col))) % m_q == 0)
			{
				matrixA(row, col) = 1;
			}
		}
	}

	return matrixA;
}

auto sortHelper(const std::vector<arma::uword>& i, const std::vector<arma::uword>& j) -> bool
{
	return i.at(0) < j.at(0);
}

auto LinearCodeGenerator::generateRepresentGroups(const std::unordered_map<arma::uword, std::vector<arma::uword>>& cycle,
	int vecCount) const -> std::vector<std::vector<arma::uword>>
{
	std::vector<std::vector<arma::uword>> repGroups;
	repGroups.reserve(cycle.size());
	for (auto& c : cycle)
	{
		repGroups.push_back(c.second);
	}

	for (auto i = 0; i < vecCount; ++i)
	{
		auto found = false;
		for (auto& repGroup : repGroups)
		{
			for (int k : repGroup)
			{
				if (k == i)
				{
					found = true;
				}
			}
		}

		if (!found)
		{
			repGroups.push_back(std::vector<arma::uword>{static_cast<arma::uword>(i)});
		}
	}

	std::sort(repGroups.begin(), repGroups.end(), sortHelper);

	return repGroups;
}

auto LinearCodeGenerator::generateMatrixE0() const -> arma::s32_mat
{
	std::default_random_engine generator;
	const std::uniform_int_distribution<int> distribution(0, m_q - 1);

	arma::mat e(m_k, m_k);
	do
	{
		for (auto i = 0; i < m_k; ++i)
		{
			for (auto j = 0; j < m_k; ++j)
			{
				generator.seed(std::random_device()());
				e(i, j) = distribution(generator);
			}
		}
	} while (0 == arma::det(e));

	arma::s32_mat e0(m_k, m_k);
	for (auto i = 0; i < m_k; ++i)
	{
		for (auto j = 0; j < m_k; ++j)
		{
			e0(i, j) = e(i, j);
		}
	}

	return e0;
}

auto LinearCodeGenerator::cycleCheck(const std::vector<arma::s32_vec>& rVector, const arma::s32_mat& E, uint16_t q) const
-> std::unordered_map<arma::uword, std::vector<arma::uword>>
{
	std::unordered_map<arma::uword, arma::uword> convert;
	for (arma::uword i = 0; i < rVector.size(); i++)
	{
		arma::s32_vec tmpVec = E * rVector.at(i);
		Math::normalizeVector(tmpVec, q);
		if (!Math::isLinDependent(rVector.at(i), tmpVec, q))
		{
			const auto vectorIndex = Math::getVectorIndex(rVector, tmpVec, q);
			if (-1 != vectorIndex)
			{
				//cout << "Found conversion: r" << i << " -> r" << vectorIndex << endl;
				convert.insert({ i, vectorIndex });
			}
		}
	}

	std::unordered_map<arma::uword, std::vector<arma::uword>> cycles;
	for (arma::uword i = 0; i < rVector.size(); i++)
	{
		const auto key = convert.find(i);
		if (key != convert.end())
		{
			std::vector<arma::uword> tmp;
			tmp.push_back(key->first);
			tmp.push_back(key->second);

			bool search;
			do
			{
				search = false;
				const auto next = convert.find(tmp.back());
				if (next != convert.end())
				{
					const auto loop = find(tmp.begin() + 1, tmp.end(), next->second);
					if (loop != tmp.end())
					{
						tmp.erase(tmp.begin(), loop);
					}

					tmp.push_back(next->second);
					search = true;
				}
			} while (search && tmp.front() != tmp.back());

			if (tmp.front() == tmp.back())
			{
				tmp.pop_back();
				std::sort(tmp.begin(), tmp.end());
				if (cycles.find(tmp.front()) == cycles.end())
				{
					cycles.insert({ tmp.front(), tmp });
				}
			}
		}
	}

	return cycles;
}


#ifndef BASETYPES
#define BASETYPES

/** \file BaseTypes.hpp
 * \brief Basic type definitions and global constants */

#include <armadillo>

using Matrix = arma::mat;
using Vector = arma::vec;
using Vector_un = arma::Col<unsigned>;

namespace math
{
const double pi = arma::datum::pi;
const double rpi2 = 1.0 / (2.0 * pi);
};

#endif


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestPoint3D
#include <boost/test/unit_test.hpp>
#include "minimath/point3d.hpp"
#include "minimath/point3d_ops.hpp"
#include "minimath/numeric_utils.hpp" // for fp comparisons
#include <iostream>
#include <sstream>
#include <limits>
#include <cmath>

namespace
{
typedef minimath::pointxyzd::value_type scalar_type;
const scalar_type EPS = std::numeric_limits<scalar_type>::epsilon();
}

BOOST_AUTO_TEST_SUITE(TestPoint3D)

BOOST_AUTO_TEST_CASE(testInstantiation)
{
  minimath::pointxyzd pd1;
  minimath::pointxyzd pd2(1,2,3);
}

BOOST_AUTO_TEST_CASE(testEquality)
{
  BOOST_CHECK(minimath::pointxyzd()==minimath::pointxyzd());
  minimath::pointxyzd pd1(1, 2, 3);
  minimath::pointxyzd pd2(1,2,3);
  BOOST_CHECK( pd1 == pd2);
}

BOOST_AUTO_TEST_CASE(testInequality)
{
  minimath::pointxyzd pd1(10, 20, 30);
  minimath::pointxyzd pd2(1,2,3);
  BOOST_CHECK( pd1 != pd2);
}

BOOST_AUTO_TEST_CASE(testCloseInequality)
{
  minimath::pointxyzd pd1(10, 20, 30);
  minimath::pointxyzd pd2(10.00001,20,30);
  BOOST_CHECK( pd1 != pd2);
}

BOOST_AUTO_TEST_CASE(testAccessX)
{
  minimath::pointxyzd pd1(10, 20, 30);
  BOOST_CHECK( minimath::compare_with_tolerance(pd1.x(), 10., EPS));
}

BOOST_AUTO_TEST_CASE(testAccessY)
{
  minimath::pointxyzd pd1(10, 20, 30);
  BOOST_CHECK( minimath::compare_with_tolerance(pd1.y(), 20., EPS) );
}

BOOST_AUTO_TEST_CASE(testAccessZ)
{
  minimath::pointxyzd pd1(10, 20, 30);
  BOOST_CHECK( minimath::compare_with_tolerance(pd1.z(), 30., EPS) );
}

BOOST_AUTO_TEST_CASE(testSetX)
{
  minimath::pointxyzd pd1;
  pd1.x(3.14);
  BOOST_CHECK( minimath::compare_with_tolerance(pd1.x(), 3.14, EPS) );
}

BOOST_AUTO_TEST_CASE(testSetY)
{
  minimath::pointxyzd pd1;
  pd1.y(3.14);
  BOOST_CHECK(  minimath::compare_with_tolerance(pd1.y(), 3.14, EPS) );
}

BOOST_AUTO_TEST_CASE(testSetZ)
{
  minimath::pointxyzd pd1;
  pd1.z(3.14);
  BOOST_CHECK(  minimath::compare_with_tolerance(pd1.z(), 3.14, EPS) );
}

BOOST_AUTO_TEST_CASE(testCopyConstruction)
{
  minimath::pointxyzd pd1(1,2,3);
  minimath::pointxyzd pd2 = pd1;
  BOOST_CHECK( pd1 == pd2);
}

BOOST_AUTO_TEST_CASE(testAssignment)
{
  minimath::pointxyzd pd1(1,2,3);
  minimath::pointxyzd pd2;
  pd2 = pd1;
  BOOST_CHECK( pd1 == pd2);
}

BOOST_AUTO_TEST_CASE(testPlusEquals)
{
  minimath::pointxyzd pd(1, 2, 3);
  pd += minimath::pointxyzd(10,20,30);
  BOOST_CHECK( pd == minimath::pointxyzd(11,22,33));
}

BOOST_AUTO_TEST_CASE(testMinusEquals)
{
  minimath::pointxyzd pd(11, 22, 33);
  pd -= minimath::pointxyzd(10,20,30);
  BOOST_CHECK( pd == minimath::pointxyzd(1,2,3));
}

BOOST_AUTO_TEST_CASE(testTimesEqualsScalar)
{
  minimath::pointxyzd pd(1, 2, 3);
  pd *= 11;
  BOOST_CHECK( pd == minimath::pointxyzd(11,22,33));

}

BOOST_AUTO_TEST_CASE(testDivideEqualsScalar)
{
  minimath::pointxyzd pd(100, 200, 300);
  pd /= 10;
  BOOST_CHECK( pd == minimath::pointxyzd(10,20,30));
}

BOOST_AUTO_TEST_CASE(testTimesScalar)
{
  minimath::pointxyzd pd = minimath::pointxyzd(1, 2, 3) * 11;
  BOOST_CHECK( pd == minimath::pointxyzd(11,22,33));

}

BOOST_AUTO_TEST_CASE(testDivideScalar)
{
  minimath::pointxyzd pd = minimath::pointxyzd(100, 200, 300)/10;
  BOOST_CHECK( pd == minimath::pointxyzd(10,20,30));
}

BOOST_AUTO_TEST_CASE(testMagnitudeSquared)
{
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::mag2(minimath::pointxyzd(1,1,1)), 3., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::mag2(minimath::pointxyzd(5,5,5)), 75., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::mag2(minimath::pointxyzd(5,0,0)), 25., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::mag2(minimath::pointxyzd(0,5,0)), 25., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::mag2(minimath::pointxyzd(0,0,5)), 25., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::mag2(minimath::pointxyzd(-5,0,5)), 50., EPS));
}

BOOST_AUTO_TEST_CASE(testDistanceSquared)
{
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(1,1,1),
                                                        minimath::pointxyzd(1,1,1)), 0., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(1,1,1),
                                                        minimath::pointxyzd(0,0,0)), 3., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(1,1,0),
                                                        minimath::pointxyzd(0,0,0)), 2., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(1,0,1),
                                                        minimath::pointxyzd(0,0,0)), 2., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(1,0,0),
                                                        minimath::pointxyzd(0,0,0)), 1., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(0,1,0),
                                                        minimath::pointxyzd(0,0,0)), 1., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(0,0,1),
                                                        minimath::pointxyzd(0,0,0)), 1., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(1,0,0),
                                                        minimath::pointxyzd(-1,0,0)), 4., EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(minimath::dist2(minimath::pointxyzd(1,1,1),
                                                        minimath::pointxyzd(-1,-1,-1)), 12., EPS));
}

BOOST_AUTO_TEST_CASE(testDotProduct)
{
  BOOST_CHECK(minimath::compare_with_tolerance(3., minimath::dot(minimath::pointxyzd(1,1,1),
                                                          minimath::pointxyzd(1,1,1)), EPS));
  // parallel opposite direction
  BOOST_CHECK(minimath::compare_with_tolerance(-3., minimath::dot(minimath::pointxyzd(1,1,1),
                                                           minimath::pointxyzd(-1,-1,-1)), EPS));
  // orthoginal
  BOOST_CHECK(minimath::compare_with_tolerance(0., minimath::dot(minimath::pointxyzd(1,0,0),
                                                          minimath::pointxyzd(0,1,0)), EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(0., minimath::dot(minimath::pointxyzd(1,0,0),
                                                          minimath::pointxyzd(0,0,1)), EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(0., minimath::dot(minimath::pointxyzd(0,1,0),
                                                          minimath::pointxyzd(1,0,0)), EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(0., minimath::dot(minimath::pointxyzd(0,1,0),
                                                          minimath::pointxyzd(0,0,1)), EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(0., minimath::dot(minimath::pointxyzd(0,0,1),
                                                          minimath::pointxyzd(1,0,0)), EPS));
  BOOST_CHECK(minimath::compare_with_tolerance(0., minimath::dot(minimath::pointxyzd(0,0,1),
                                                          minimath::pointxyzd(0,1,0)), EPS));
}

namespace
{
void test_normalize_non_member(int i, int j, int k)
{
  using namespace minimath;
  for (int n = 0; n < 100; ++n)
  {
    pointxyzd p(i*std::rand(), j*std::rand(), k*std::rand());
    pointxyzd::value_type d = std::sqrt(mag2(p));
    pointxyzd::value_type EPS = std::numeric_limits<minimath::pointxyzd::value_type>::epsilon();
    p = normalize(p);
    d = mag2(p);
    BOOST_CHECK(compare_with_tolerance(d, pointxyzd::value_type(1.), 5.0e-16));
  }
}
void test_normalize(int i, int j, int k)
{
  using namespace minimath;
  for (int n = 0; n < 100; ++n)
  {
    pointxyzd p(i*std::rand(), j*std::rand(), k*std::rand());
    pointxyzd::value_type d = std::sqrt(mag2(p));
    pointxyzd::value_type EPS = std::numeric_limits<minimath::pointxyzd::value_type>::epsilon();
    BOOST_CHECK(compare_with_tolerance(p.normalize(), d, EPS));
    std::stringstream ss;
    ss << "mag2(p) -1 = " << mag2(p) - pointxyzd::value_type(1.);
    ss << " EPS " << EPS;
    d = mag2(p);
    BOOST_CHECK_MESSAGE(ss.str().c_str(), compare_with_tolerance(d, pointxyzd::value_type(1.), 5.0e-16));
  }
}
}

BOOST_AUTO_TEST_CASE(testNormalize)
{
    test_normalize(1, 1, 1);
    test_normalize(1, 1, -1);
    test_normalize(1, -1, 1);
    test_normalize(-1, 1, 1);
    test_normalize(1, -1, -1);
    test_normalize(-1, -1, 1);
    test_normalize(-1, 1, -1);
    test_normalize(-1, -1, -1);
}

BOOST_AUTO_TEST_CASE(testNormalizeNonMember)
{
    test_normalize_non_member(1, 1, 1);
    test_normalize_non_member(1, 1, -1);
    test_normalize_non_member(1, -1, 1);
    test_normalize_non_member(-1, 1, 1);
    test_normalize_non_member(1, -1, -1);
    test_normalize_non_member(-1, -1, 1);
    test_normalize_non_member(-1, 1, -1);
    test_normalize_non_member(-1, -1, -1);
}

BOOST_AUTO_TEST_SUITE_END()


#include "acrolib/sampling.h"

#include <Eigen/Core>

namespace acro
{
template <typename SampleType>
GridSamples<SampleType>::GridSamples(const std::vector<std::vector<double>>& sample_ranges)
  : sample_ranges_(sample_ranges), n_(sample_ranges.size())
{
  for (auto& r : sample_ranges_)
    sample_range_sizes_.push_back(r.size());

  for (auto& s : sample_range_sizes_)
    total_sample_count_ *= s;
}

template <typename SampleType>
SampleType GridSamples<SampleType>::operator[](std::size_t ind) const
{
  SampleType sample(n_);
  auto s = convertBase(ind, sample_range_sizes_);
  for (std::size_t dim{ 0 }; dim < n_; ++dim)
  {
    sample[dim] = sample_ranges_.at(dim).at(s[dim]);
  }
  return sample;
}

template class GridSamples<Eigen::VectorXd>;
template class GridSamples<std::vector<double>>;

std::vector<std::vector<double>> calculateRangesFromLimits(const std::vector<Limits>& limits, double resolution)
{
  // calculate the specific sample levels for each dimension
  std::vector<std::vector<double>> sample_ranges(limits.size());
  for (size_t k{ 0 }; k < limits.size(); ++k)
  {
    double value = limits.at(k).lower;
    while (value <= limits.at(k).upper)
    {
      sample_ranges.at(k).push_back(value);
      value += resolution;
    }
  }

  return sample_ranges;
}

}  // namespace acro


/***************************************************************************
                          meshalgs.cc  -  description
                             -------------------
    begin                : Mon Mar 31 2008
    copyright            : (C) 2005 by Knut-Helge Vik
    email                : knuthelv@ifi.uio.no
 ***************************************************************************/
#include "meshalgs.h"
#include "../treealgs/treestructure.h"
#include "../simdefs.h"
#include "../treealgs/fheap.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graph_utility.hpp>
#include <functional>
#include <iostream>
#include <fstream>
#include <cmath>

using namespace std;
using namespace boost;
using namespace TreeAlgorithms;

//typedef TreeAlgorithms::COLOR COLOR;

namespace GraphAlgorithms
{

// dijkstra's shortest path tree
void maxDistance(const GraphN &g, const VertexSet &V, int src, DistanceVector &zdistance, DistanceVector &hops, ParentVector &zparent, double &eccentricity, int &node_in_eccentricity_path)
{
    out_edge_iteratorN oit, oit_end;
    //enum COLOR { WHITE = 0, GRAY, BLACK};
    std::vector<TreeAlgorithms::COLOR> colorVector(num_vertices(g));

    HeapD<FHeap> heapD;
    Heap *heap = heapD.newInstance(num_vertices(g));

    for(VertexSet::iterator vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
    {
        zdistance[*vit] = (std::numeric_limits<double>::max)();
        zparent[*vit] = *vit;
        hops[*vit] = 0;
        colorVector[*vit] = WHITE;
    }

    colorVector[src] = GRAY;
    zdistance[src] = 0;
    heap->insert(src, 0.0);

    while(heap->nItems() > 0)
    {
        int u = heap->deleteMin();
        for(boost::tuples::tie(oit, oit_end) = out_edges(u, g); oit != oit_end; ++oit)
        {
            int v = target(*oit, g);
            double path_weight = zdistance[u] + g[*oit].weight;

            if((path_weight < zdistance[v]))
            {
                zdistance[v] = path_weight;
                zparent[v] = u;
                hops[v] = hops[u] + 1;

                if(colorVector[v] == WHITE)
                {
                    colorVector[v] = GRAY;
                    heap->insert(v, zdistance[v]);
                }
                else if(colorVector[v] == GRAY)
                {
                    heap->decreaseKey(v, zdistance[v]);
                }

                if(zdistance[v] >= eccentricity)
                {
                    eccentricity = zdistance[v];
                    node_in_eccentricity_path = v;
                }
            }
        }
        colorVector[u] = BLACK;
    }
    delete heap;
}

void maxDistance(const TreeStructure &T, int src, DistanceVector &zdistance, DistanceVector &hops, ParentVector &zparent, double &eccentricity, int &node_in_eccentricity_path)
{
    maxDistance(T.g, T.V, src, zdistance, hops, zparent, eccentricity, node_in_eccentricity_path);
}

// dijkstra's shortest path tree
void maxDistance(const TreeStructure &T, int src, DistanceVector &zdistance, DistanceVector &hops, ParentVector &zparent)
{
    double ecc = 0;
    int node = -1;

    maxDistance(T, src, zdistance, hops, zparent,ecc, node);
}

double eccentricity_distance(const TreeStructure &T, int src)
{
    return eccentricity_distance(T.g, T.V, src);
}

double eccentricity_distance(const GraphN &g, const VertexSet &V, int src)
{
    DistanceVector 	distance(num_vertices(g));
    DistanceVector 	hops(num_vertices(g));
    ParentVector	parent(num_vertices(g));
    double ecc = 0;
    int node = -1;

    maxDistance(g, V, src, distance, hops, parent, ecc, node);

    return ecc;
}

double eccentricity_distance(const GraphN &g, const VertexSet &V, int src, DistanceVector &distance, DistanceVector &hops, ParentVector &parent)
{
    double ecc = 0;
    int node = -1;

    maxDistance(g, V, src, distance, hops, parent, ecc, node);
    return ecc;
}

// breadth first search tree
void maxDistanceHops(const TreeStructure &T, int src, DistanceVector &distance, DistanceVector &hops, ParentVector &parent)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        distance[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        hops[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        parent[*vit] = *vit;
    }

    distance[src] = 0;
    hops[src] = 0;
    parent[src] = src;

    typedef list<int> IntList;
    IntList neighbors;
    neighbors.push_back(src);

    while(!neighbors.empty())
    {
        int v = neighbors.front();
        neighbors.pop_front();

        ASSERTING(T.V.contains(v));
        out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
        {
            int u = target(*oit, g);
            ASSERTING(T.V.contains(u));

            if(hops[u] == (std::numeric_limits<double>::max()))
            {
                hops[u] = hops[v] + 1;
                distance[u] = distance[v] + g[*oit].weight;
                parent[u] = v;
                neighbors.push_back(u);
            }
        }
    }
}

// breadth first search tree
double maxDistanceHops(const TreeStructure &T, int src, DistanceVector &distance)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
        distance[*vit] = (std::numeric_limits<double>::max)();

    distance[src] = 0;
    double diameter = 0;

    typedef list<int> IntList;
    IntList neighbors;
    neighbors.push_back(src);

    while(!neighbors.empty())
    {
        int v = neighbors.front();
        neighbors.pop_front();

        out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
        {
            int u = target(*oit, g);
            ASSERTING(T.V.contains(u));

            if(distance[u] == (std::numeric_limits<double>::max()))
            {
                distance[u] = distance[v] + 1;
                if(distance[u] > diameter) diameter = distance[u];

                neighbors.push_back(u);
            }
        }
    }
    return diameter;
}

double eccentricity_hops(const TreeStructure &T, int src)
{
    DistanceVector 	distance(num_vertices(T.g));

    return maxDistanceHops(T, src, distance);
}

double diameter_hops(const TreeStructure &T)
{
    double max = 0;
    DistanceVector distance(num_vertices(T.g));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        int v = *vit;
        double d = maxDistanceHops(T, v, distance);
        max = d > max ? d : max;
    }

    return max;
}

double diameter_distance(const TreeStructure &T)
{
    double diameter = 0;
    PathList diameterPath;
    diameter_distance(T, diameterPath, diameter);

    return diameter;
}

void diameter_distance(const TreeStructure &T, PathList &diameterPath, double &diameter)
{
    DistanceVector 	distance(num_vertices(T.g));
    DistanceVector 	hops(num_vertices(T.g));
    ParentVector	parent(num_vertices(T.g));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        double eccentricity = 0;
        int node_in_eccentricity_path = -1;
        int v = *vit;
        maxDistance(T, v, distance, hops, parent, eccentricity, node_in_eccentricity_path);

        if(diameter < eccentricity)
        {
            diameter = eccentricity;
            diameterPath.clear();

            ASSERTING(node_in_eccentricity_path != v);
            int path_node = node_in_eccentricity_path;
            while(path_node != v)
            {
                diameterPath.push_back(path_node);
                path_node = parent[path_node];
            }

            diameterPath.push_back(v);

            //cerr << WRITE_FUNCTION << " diameter : " << diameter << endl;
        }
    }
}

// matrix of the shortest distances between any two vertices.
void distanceMatrix(const TreeStructure &T, DistanceVectorMatrix &distance, DistanceVectorMatrix &hops, ParentVectorMatrix &parent)
{
    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        int v = *vit;
        maxDistance(T, v, distance[v], hops[v], parent[v]);
    }
}

void distanceMatrixHops(const TreeStructure &T, DistanceVectorMatrix &distance)
{
    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        int v = *vit;
        maxDistanceHops(T, v, distance[v]);
    }
}

void printAllPairsSP(ParentMatrix &parent, int src, int targ, PathList &path)
{
    if(src == targ)
    {
        cerr << src << " ";
        path.push_back(src);
    }
    else if(parent[src][targ] == src)
    {
        cerr << " no path from " << src << " to " << targ << " exists " << endl;
    }
    else
    {
        printAllPairsSP(parent, src, parent[src][targ], path);
        cerr << targ << " ";
        path.push_back(targ);
    }
}

};

/*
double maxDistanceHops(const TreeStructure &T, int src, DistanceVector &distance)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
        distance[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance

    distance[src] = 0;
    double depth = 1;                       // next distance in BFS
    int cnt = 0;

    VertexSet neighborSet;
    neighborSet.insert(src);

    while(!neighborSet.empty())
    {
        VertexSet nextNeighbors;
        for(VertexSet::iterator vit = neighborSet.begin(), vit_end = neighborSet.end(); vit != vit_end; ++vit)
        {
            int v = *vit;
            out_edge_iteratorN oit, oit_end;
            ASSERTING(T.V.contains(v));

            for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
            {
                int u = target(*oit, g);
                if(distance[u] == (std::numeric_limits<double>::max)()) // first time reachable?
                {
                    cnt++;
                    distance[u] = depth;
                    nextNeighbors.insert(u);
                }
            }
        }
        // next try set of vertices further away
        neighborSet = nextNeighbors;
        depth++;
    }

    return cnt == T.V.size() ? depth-1 : T.V.size();
}
*/

/*void maxDistance(const TreeStructure &T, int src, DistanceVector &distance, DistanceVector &hops, ParentVector &parent, double &eccentricity, int &node_in_eccentricity_path)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        distance[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        hops[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        parent[*vit] = *vit;
    }

    distance[src] = 0;
    hops[src] = 0;
    parent[src] = src;

    typedef list<int> IntList;
    IntList neighbors;
    neighbors.push_back(src);

    while(!neighbors.empty())
    {
        int v = neighbors.front();
        neighbors.pop_front();

        ASSERTING(T.V.contains(v));
        out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
        {
            int u = target(*oit, g);
            ASSERTING(T.V.contains(u));

            if(distance[u] > distance[v] + g[*oit].weight)
            {
                hops[u] = hops[v] + 1;
                distance[u] = distance[v] + g[*oit].weight;
                parent[u] = v;
                neighbors.push_back(u);

                if(distance[u] >= eccentricity)
                {
                    eccentricity = distance[u];
                    node_in_eccentricity_path = u;
                }
                //cerr << u << " hops " << hops[u] << " distance " << distance[u] << " parent " << parent[u] << endl;
            }
        }
    }
}*/


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object returns \f$\sqrt{1+x}-1\f$ and the
    result is accurate even for x  with small modulus

    @see log1p, expm1.

    @par Header <boost/simd/function/sqrt1pm1.hpp>

    @par Example:

      @snippet sqrt1pm1.cpp sqrt1pm1

    @par Possible output:

      @snippet sqrt1pm1.txt sqrt1pm1
  **/
  IEEEValue sqrt1pm1(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sqrt1pm1.hpp>
#include <boost/simd/function/simd/sqrt1pm1.hpp>

#endif


/**
 * @file cmaes_test.cpp
 * @author Marcus Edel
 * @author Kartik Nighania
 *
 * Test file for CMA-ES.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/cmaes/cmaes.hpp>
#include <mlpack/core/optimizers/problems/sgd_test_function.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace arma;
using namespace mlpack::optimization;
using namespace mlpack::optimization::test;

using namespace mlpack::distribution;
using namespace mlpack::regression;

using namespace mlpack;

BOOST_AUTO_TEST_SUITE(CMAESTest);

/**
 * Tests the CMA-ES optimizer using a simple test function.
 */
BOOST_AUTO_TEST_CASE(SimpleTestFunction)
{
  SGDTestFunction f;
  CMAES<> optimizer(0, -1, 1, 32, 200, -1);

  arma::mat coordinates = f.GetInitialPoint();
  optimizer.Optimize(f, coordinates);

  BOOST_REQUIRE_SMALL(coordinates[0], 0.003);
  BOOST_REQUIRE_SMALL(coordinates[1], 0.003);
  BOOST_REQUIRE_SMALL(coordinates[2], 0.003);
}

/**
 * Create the data for the logistic regression test case.
 */
void CreateLogisticRegressionTestData(arma::mat& data,
                                      arma::mat& testData,
                                      arma::mat& shuffledData,
                                      arma::Row<size_t>& responses,
                                      arma::Row<size_t>& testResponses,
                                      arma::Row<size_t>& shuffledResponses)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  data = arma::mat(3, 1000);
  responses = arma::Row<size_t>(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0,
      data.n_cols - 1, data.n_cols));
  shuffledData = arma::mat(3, 1000);
  shuffledResponses = arma::Row<size_t>(1000);
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    shuffledData.col(i) = data.col(indices[i]);
    shuffledResponses[i] = responses[indices[i]];
  }

  // Create a test set.
  testData = arma::mat(3, 1000);
  testResponses = arma::Row<size_t>(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    testData.col(i) = g1.Random();
    testResponses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    testData.col(i) = g2.Random();
    testResponses[i] = 1;
  }
}

/**
 * Run CMA-ES with the full selection policy on logistic regression and
 * make sure the results are acceptable.
 */
BOOST_AUTO_TEST_CASE(CMAESLogisticRegressionTest)
{
  const size_t trials = 3;
  bool success = false;
  for (size_t trial = 0; trial < trials; ++trial)
  {
    arma::mat data, testData, shuffledData;
    arma::Row<size_t> responses, testResponses, shuffledResponses;

    CreateLogisticRegressionTestData(data, testData, shuffledData,
        responses, testResponses, shuffledResponses);

    CMAES<> cmaes(0, -1, 1, 32, 200, 1e-3);
    LogisticRegression<> lr(shuffledData, shuffledResponses, cmaes, 0.5);

    // Ensure that the error is close to zero.
    const double acc = lr.ComputeAccuracy(data, responses);
    const double testAcc = lr.ComputeAccuracy(testData, testResponses);
    if (acc >= 99.7 && testAcc >= 99.4)
    {
      success = true;
      break;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

/**
 * Run CMA-ES with the random selection policy on logistic regression and
 * make sure the results are acceptable.
 */
BOOST_AUTO_TEST_CASE(ApproxCMAESLogisticRegressionTest)
{
  const size_t trials = 3;
  bool success = false;
  for (size_t trial = 0; trial < trials; ++trial)
  {
    arma::mat data, testData, shuffledData;
    arma::Row<size_t> responses, testResponses, shuffledResponses;

    CreateLogisticRegressionTestData(data, testData, shuffledData,
        responses, testResponses, shuffledResponses);

    ApproxCMAES<> cmaes(0, -1, 1, 32, 200, 1e-3);
    LogisticRegression<> lr(shuffledData, shuffledResponses, cmaes, 0.5);

    // Ensure that the error is close to zero.
    const double acc = lr.ComputeAccuracy(data, responses);
    const double testAcc = lr.ComputeAccuracy(testData, testResponses);
    if (acc >= 99.7 && testAcc >= 99.4)
    {
      success = true;
      break;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

BOOST_AUTO_TEST_SUITE_END();


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* EncryptedArray.cpp - Data-movement operations on arrays of slots
 */
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <helib/EncryptedArray.h>
#include <helib/polyEval.h>
#include <helib/debugging.h>

namespace helib {

// Compute a degree-p polynomial poly(x) s.t. for any t<e and integr z of the
// form z = z0 + p^t*z1 (with 0<=z0<p), we have poly(z) = z0 (mod p^{t+1}).
//
// We get poly(x) by interpolating a degree-(p-1) polynomial poly'(x)
// s.t. poly'(z0)=z0 - z0^p (mod p^e) for all 0<=z0<p, and then setting
// poly(x) = x^p + poly'(x).
static void buildDigitPolynomial(NTL::ZZX& result, long p, long e)
{
  if (p<2 || e<=1) return; // nothing to do
  FHE_TIMER_START;
  long p2e = NTL::power_long(p,e); // the integer p^e

  // Compute x - x^p (mod p^e), for x=0,1,...,p-1
  NTL::vec_long x(NTL::INIT_SIZE, p);
  NTL::vec_long y(NTL::INIT_SIZE, p);
  long bottom = -(p/2);
  for (long j=0; j<p; j++) {
    long z = bottom+j;
    x[j] = z;
    y[j] = z-NTL::PowerMod((z < 0 ? z + p2e : z), p, p2e);  // x - x^p (mod p^e)

    while (y[j] > p2e/2)         y[j] -= p2e;
    while (y[j] < -(p2e/2))      y[j] += p2e;
  }
  interpolateMod(result, x, y, p, e);
  //OLD: assert(deg(result)<p); // interpolating p points, should get deg<=p-1
  helib::assertTrue(deg(result)<p, "Interpolation error - degree too high");
  SetCoeff(result, p);   // return result = x^p + poly'(x)
  //  cerr << "# digitExt mod "<<p<<"^"<<e<<"="<<result<<endl;
  FHE_TIMER_STOP;
}

// extractDigits assumes that the slots of *this contains integers mod p^r
// i.e., that only the free terms are nonzero. (If that assumptions does
// not hold then the result will not be a valid ciphertext anymore.)
//
// It returns in the slots of digits[j] the j'th-lowest gigits from the
// integers in the slots of the input. Namely, the i'th slot of digits[j]
// contains the j'th digit in the p-base expansion of the integer in the
// i'th slot of the *this. The plaintext space of digits[j] is mod p^{r-j},
// and all the digits are at the same level.

int fhe_watcher = 0;

void extractDigits(std::vector<Ctxt>& digits, const Ctxt& c, long r)
{
  const Context& context = c.getContext();
  long rr = c.effectiveR();
  if (r<=0 || r>rr) r = rr; // how many digits to extract

  long p = context.zMStar.getP();

  NTL::ZZX x2p;
  if (p>3) {
    buildDigitPolynomial(x2p, p, r);
  }

  Ctxt tmp(c.getPubKey(), c.getPtxtSpace());
  digits.resize(r, tmp);      // allocate space

#ifdef DEBUG_PRINTOUT
  fprintf(stderr, "***\n");
#endif
  for (long i=0; i<r; i++) {
    tmp = c;
    for (long j=0; j<i; j++) {

      if (p==2) digits[j].square();
      else if (p==3) digits[j].cube();
      else polyEval(digits[j], x2p, digits[j]);
      // "in spirit" digits[j] = digits[j]^p

#ifdef DEBUG_PRINTOUT
      fprintf(stderr, "%5ld", digits[j].bitCapacity());
#endif

      tmp -= digits[j];
      tmp.divideByP();
    }
    digits[i] = tmp; // needed in the next round

#ifdef DEBUG_PRINTOUT
    if (dbgKey) {
       double ratio =
          log(embeddingLargestCoeff(digits[i], *dbgKey)/digits[i].getNoiseBound())/log(2.0);
       fprintf(stderr, "%5ld [%f]", digits[i].bitCapacity(), ratio);
       if (ratio > 0) fprintf(stderr, " BAD-BOUND");
       fprintf(stderr, "\n");
    }
    else {
       fprintf(stderr, "%5ld\n", digits[i].bitCapacity());
    }
#endif
  }

#ifdef DEBUG_PRINTOUT
  fprintf(stderr, "***\n");
#endif
}

static
void compute_a_vals(NTL::Vec<NTL::ZZ>& a, long p, long e)
// computes a[m] = a(m)/m! for m = p..(e-1)(p-1)+1,
// as defined by Chen and Han.
// a.length() is set to (e-1)(p-1)+2

{
   NTL::ZZ p_to_e = NTL::power_ZZ(p, e);
   NTL::ZZ p_to_2e = NTL::power_ZZ(p, 2*e);

   long len = (e-1)*(p-1)+2;

   NTL::ZZ_pPush push(p_to_2e);

   NTL::ZZ_pX x_plus_1_to_p = power(NTL::ZZ_pX(NTL::INIT_MONO, 1) + 1, p);
   NTL::ZZ_pX denom = InvTrunc(x_plus_1_to_p - NTL::ZZ_pX(NTL::INIT_MONO, p), len);
   NTL::ZZ_pX poly = MulTrunc(x_plus_1_to_p, denom, len);
   poly *= p;

   a.SetLength(len);

   NTL::ZZ m_fac(1);
   for (long m = 2; m < p; m++) {
      m_fac = MulMod(m_fac, m, p_to_2e);
   }

   for (long m = p; m < len; m++) {
      m_fac = MulMod(m_fac, m, p_to_2e);
      NTL::ZZ c = rep(coeff(poly, m));
      NTL::ZZ d = GCD(m_fac, p_to_2e);
      if (d == 0 || d > p_to_e || c % d != 0) throw helib::RuntimeError("cannot divide");
      NTL::ZZ m_fac_deflated = (m_fac / d) % p_to_e;
      NTL::ZZ c_deflated = (c / d) % p_to_e;
      a[m] = MulMod(c_deflated, InvMod(m_fac_deflated, p_to_e), p_to_e);
   }

}

// This computes Chen and Han's magic polynomial G, which
// has the property that G(x) = (x mod p) (mod p^e).
// Here, (x mod p) is in the interval [0,1] if p == 2,
// and otherwise, is in the interval (-p/2, p/2).
static
void compute_magic_poly(NTL::ZZX& poly1, long p, long e)
{
   FHE_TIMER_START;

   NTL::Vec<NTL::ZZ> a;

   compute_a_vals(a, p, e);

   NTL::ZZ p_to_e = NTL::power_ZZ(p, e);
   long len = (e-1)*(p-1)+2;

   NTL::ZZ_pPush push(p_to_e);

   NTL::ZZ_pX poly(0);
   NTL::ZZ_pX term(1);
   NTL::ZZ_pX X(NTL::INIT_MONO, 1);

   poly = 0;
   term = 1;

   for (long m = 0; m < p; m++) {
      term *= (X-m);
   }

   for (long m = p; m < len; m++) {
      poly += term * NTL::conv<NTL::ZZ_p>(a[m]);
      term *= (X-m);
   }

   // replace poly by poly(X+(p-1)/2) for odd p
   if (p % 2 == 1) {
      NTL::ZZ_pX poly2(0);

      for (long i = deg(poly); i >= 0; i--)
         poly2 = poly2*(X+(p-1)/2) + poly[i];

      poly = poly2;
   }

   poly = X - poly;
   poly1 = NTL::conv<NTL::ZZX>(poly);
}

// extendExtractDigits assumes that the slots of *this contains integers mod
// p^{r+e} i.e., that only the free terms are nonzero. (If that assumptions
// does not hold then the result will not be a valid ciphertext anymore.)
//
// It returns in the slots of digits[j] the j'th-lowest digits from the
// integers in the slots of the input. Namely, the i'th slot of digits[j]
// contains the j'th digit in the p-base expansion of the integer in the i'th
// slot of the *this.  The plaintext space of digits[j] is mod p^{e+r-j}.

void extendExtractDigits(std::vector<Ctxt>& digits, const Ctxt& c, long r, long e)
{
  const Context& context = c.getContext();

  long p = context.zMStar.getP();
  NTL::ZZX x2p;
  if (p>3) {
    buildDigitPolynomial(x2p, p, r);
  }

  // we should pre-compute this table
  // for i = 0..r-1, entry i is G_{e+r-i} in Chen and Han
  NTL::Vec<NTL::ZZX> G;
  G.SetLength(r);
  for (long i: range(r)) {
    compute_magic_poly(G[i], p, e+r-i);
  }

  std::vector<Ctxt> digits0;

  Ctxt tmp(c.getPubKey(), c.getPtxtSpace());

  digits.resize(r, tmp);      // allocate space
  digits0.resize(r, tmp);

#ifdef DEBUG_PRINTOUT
  fprintf(stderr, "***\n");
#endif
  for (long i: range(r)) {
    tmp = c;
    for (long j: range(i)) {
      if (digits[j].capacity() >= digits0[j].capacity()) {
         // optimization: digits[j] is better than digits0[j],
         // so just use it

         tmp -= digits[j];
#ifdef DEBUG_PRINTOUT
      fprintf(stderr, "%5ld*", digits[j].bitCapacity());
#endif
      }
      else {
	if (p==2) digits0[j].square();
	else if (p==3) digits0[j].cube();
	else polyEval(digits0[j], x2p, digits0[j]); // "in spirit" digits0[j] = digits0[j]^p

	tmp -= digits0[j];
#ifdef DEBUG_PRINTOUT
      fprintf(stderr, "%5ld ", digits0[j].bitCapacity());
#endif
      }
      tmp.divideByP();
    }
    digits0[i] = tmp; // needed in the next round
    polyEval(digits[i], G[i], tmp);

#ifdef DEBUG_PRINTOUT
    if (dbgKey) {
      double ratio =
        log(embeddingLargestCoeff(digits[i], *dbgKey)/digits[i].getNoiseBound())/log(2.0);
      fprintf(stderr, "%5ld  --- %5ld", digits0[i].bitCapacity(), digits[i].bitCapacity());
      fprintf(stderr, " [%f]", ratio);
      if (ratio > 0) fprintf(stderr, " BAD-BOUND");
      fprintf(stderr, "\n");
    }
    else {
      fprintf(stderr, "%5ld  --- %5ld\n", digits0[i].bitCapacity(), digits[i].bitCapacity());
    }
#endif
  }
}

}


static char help[] = "A character array that PETSc might be expecting";
#include <petscksp.h>

#include <iostream>
#include <vector>
#include <math.h>
#include <string>

#include "Input_Reader.h"
#include "Materials.h"
#include "Fem_Quadrature.h"
#include "Quadrule_New.h"
#include "Cell_Data.h"
#include "Angular_Quadrature.h"
#include "Time_Data.h"
#include "Temperature_Data.h"

#include <Eigen/Dense>
#include "Diffusion_Operator.h"

#include "Dark_Arts_Exception.h"

/**
  Goal of this unit test is to check source moment formation, and reaction matrix for SLXS Lobatto scheme
  -This is a MMS problem with a spatially varying sigma_a, constant cv , zero sig_s
*/

int main(int argc, char** argv)
{
  /// change to -1 if you want ctest to dump this output.  Otherwise this test passes always
  int val = 0;
  PetscErrorCode ierr;
  PetscMPIInt size;

  PetscInitialize(&argc,&argv,(char*)0,help);
  ierr = MPI_Comm_size(PETSC_COMM_WORLD,&size);
  CHKERRQ(ierr);
  if (size != 1)
    SETERRQ(PETSC_COMM_WORLD,1,"DARK_ARTS is written to be serial only!");

  Input_Reader input_reader;
  try
  {
    input_reader.read_xml(argv[1]);
  }
  catch(const Dark_Arts_Exception& da_exception )
  {
    da_exception.message() ;
    val = -1;
  }

  /// Initialize a Quadrule object to be able to get all of the quadrature we need
  Quadrule_New quad_fun;
  Fem_Quadrature fem_quadrature( input_reader , quad_fun);
  Cell_Data cell_data( input_reader );
  Angular_Quadrature angular_quadrature( input_reader , quad_fun );

  /// Create a Materials object that contains all opacity, heat capacity, and source objects
    Materials materials( input_reader, fem_quadrature , cell_data, angular_quadrature );
    Time_Data time_data(input_reader);
    Temperature_Data t_old(fem_quadrature, input_reader, cell_data);

    const double dt = time_data.get_dt_max();
    const double time_stage = dt + time_data.get_t_start();
    double rk_a_ii = 1.;

    const int n_p = fem_quadrature.get_number_of_interpolation_points() ;
  try{
    Diffusion_Operator diffusion_operator(input_reader, fem_quadrature, cell_data,
      materials, angular_quadrature, 1,t_old, true,1.0E-20, 1.0E-10 , 2.);

    diffusion_operator.set_time_data(dt, time_stage, rk_a_ii);

    diffusion_operator.dump_matrix();

    std::vector<double> ref_norm(1,0.);
    Intensity_Moment_Data phi_old(cell_data, angular_quadrature, fem_quadrature, ref_norm);
    Intensity_Moment_Data phi_new(phi_old);

    Eigen::VectorXd phi_old_vec = Eigen::VectorXd::Zero(n_p);
    Eigen::VectorXd phi_new_vec = Eigen::VectorXd::Zero(n_p);
    for(int el = 0; el < n_p ;el++)
    {
        phi_old_vec(el) = 0.1;
        phi_new_vec(el) = 0.7;
    }

    for(int i = 0 ; i < cell_data.get_total_number_of_cells() ; i++)
    {
      phi_old.set_cell_angle_integrated_intensity(i,0,0,phi_old_vec);
      phi_new.set_cell_angle_integrated_intensity(i,0,0,phi_new_vec);
    }

    // diffusion_operator.make_and_dump_rhs(phi_new , phi_old);

    std::cout << "Update follows\n" << std::endl;
    diffusion_operator.after_rhs_solve_system_and_dump_solution();

    double sig_a = 4.0;
    double sig_s = 1.0;
    double sum_sn_w = 2.;
    double c_speed = 1.;
    double cv = 0.5;
    double temp = 0.1*(1.0);
    double d_planck = 4.*pow(temp,3)/sum_sn_w;
    double pseudo_sig_t = 1./(rk_a_ii*dt*c_speed) + sig_a + sig_s;

    double value = sum_sn_w*rk_a_ii*dt*sig_a*d_planck;
    double nu = value/(cv + value);
    double pseudo_sig_s = sig_s + nu*sig_a;

    std::cout << "Pseudo sig_s: " << pseudo_sig_s << std::endl;
    std::cout << "Pseudp sig_t: " << pseudo_sig_t << std::endl;

  }
  catch(const Dark_Arts_Exception& da)
  {
    val = -1;
    da.testing_message();
  }
  ierr = PetscFinalize();
  return val;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/pow.hpp>
#include <boost/simd/function/std.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/eight.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>

STF_CASE_TPL("pow std",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow;
  using r_t =  decltype(pow(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::std_(pow)(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(pow)(bs::Nan<T>(), bs::Nan<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(pow)(bs::Minf<T>(), bs::Minf<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(pow)(bs::Inf<T>(), bs::Minf<T>()), bs::Zero<r_t>(), 0);
#endif
  STF_ULP_EQUAL(bs::std_(pow)(T(-1),T(6)), T(1), 0);
  STF_ULP_EQUAL(bs::std_(pow)(bs::Mone<T>(), bs::Mone<T>()), bs::Mone<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(pow)(bs::One<T>(), bs::One<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(pow)(bs::Zero<T>(), bs::Zero<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(pow)(T(-1),T(5)), T(-1), 0);
  STF_ULP_EQUAL(bs::std_(pow)(bs::Zero<T>(), bs::One<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(pow)(T(8),bs::Third<T>()), r_t(2), 0.5);
}