#include <string>
#include <fstream>
#include <exception>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <ensenso/pathfinder.h>
#include <std_msgs/Float64MultiArray.h>
#include "nn_controller/nn_controller.h"

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

using namespace amfc_control;
using namespace boost::numeric::odeint;

//constructor
Controller::Controller(ros::NodeHandle nc, const Eigen::Vector3d& ref, bool print,
						bool useSigma, bool save)
: n_(nc), ref_(ref), updatePoseInfo(false), updateController(false), useSigma(useSigma),
  save(save), resetController(false), updateWeights(false), print(print), counter(0),
  multicast_address("235.255.0.1")
{
	initMatrices();
	sigma_y = 0.01;
	sigma_r = 0.01;
	// BladderTypeEnum bladder_type_;
	ros::param::get("/nn_controller/Control/with_net", with_net_);
	control_pub_ = n_.advertise<ensenso::ValveControl>("/mannequine_head/u_valves", 100);
}

//copy constructor
Controller::Controller()
{

}

// Destructor.
Controller::~Controller()
{
}

void Controller::vectorToHeadPose(Eigen::VectorXd&& pose_info, geometry_msgs::Pose& eig2Pose)
{
    eig2Pose.orientation.x = pose_info(0); // roll
    eig2Pose.position.z = pose_info(1);	// z
    eig2Pose.orientation.y = pose_info(2);	// pitch
}

ros::Time Controller::getTime() {
	return ros::Time::now();
}

/*Subscribers*/
// pose subscriber from ensenso_seg/vicon_sub
void Controller::pose_subscriber(const geometry_msgs::Pose& headPose) {
	getPoseInfo(headPose, pose_info);

	std::lock_guard<std::mutex> pose_locker(pose_mutex);
	this->pose_info = pose_info;
	updatePoseInfo  = true;
}

void Controller::net_control_subscriber(const ensenso::ValveControl& net_control_law){
	Eigen::VectorXd net_control;
	net_control.resize(6);

	net_control << net_control_law.left_bladder_pos, net_control_law.left_bladder_neg,
				   net_control_law.base_bladder_pos, net_control_law.base_bladder_neg,
				   net_control_law.right_bladder_pos, net_control_law.right_bladder_neg;
	update_net_law = true;
	this->net_control.resize(6);
	this->net_control = net_control;
}

void Controller::getPoseInfo(const geometry_msgs::Pose& headPose, Eigen::VectorXd pose_info)
{
	pose_info << headPose.orientation.x, // roll = [left and right]
				 headPose.position.z, 	// base  = [base actuator]
				 headPose.orientation.y; // pitch = [right actuator]

	this->pose_info = pose_info;
	//set ref's non-controlled states to measurement
	ControllerParams(std::move(pose_info));
}

void Controller::ControllerParams(Eigen::VectorXd&& pose_info)
{
	// Am = -0.782405        -0        -0
	//       -0          -0.782405     -0
    //       -0              -0    -0.782405
	// Bm = [1 0 0; 0 1 0; 0 0 1]
	// ref_ = [z, roll, pitch ] given by user
	if(counter == 0){
		ym = pose_info;		// will be 3x1; pose_info is also 3x1
		ym_dot = Am * ym + Bm * ref_;	// will be 3x1
		prev_ym.push_back(ym);

		tracking_error = pose_info - ym; 	// will be 3x1

		ros::param::get("/nn_controller/Utils/filename", filename_);
		pathfinder::getROSPackagePath("nn_controller", nn_controller_path_);
		ss << nn_controller_path_.c_str() << filename_;
		ref_pose_file_ = ss.str();
	}
	else{
		// find ym
		ym_dot = Am * prev_ym.back() + Bm * ref_;  // will be 3x1
		ym = prev_ym.back() + 0.01 * ym_dot;		// will be 3x1
		prev_ym.push_back(ym);    // don't leve the linked list empty

		tracking_error = pose_info - ym;			// will be 3x1

		// // find Ky_hat
		// Ky_hat_dot = -Gamma_y * pose_info * tracking_error.transpose() * P * B  * sgnLambda;
		// Ky_hat = prev_Ky_hat_.back() + 0.01 * Ky_hat_dot;
		// prev_Ky_hat_.push_back(Ky_hat);

		// // find Kr_hat
		// Kr_hat_dot = -Gamma_r * ref_      * tracking_error.transpose() * P * B  * sgnLambda;
		// Kr_hat = prev_Kr_hat_.back() + 0.01 * Kr_hat_dot;
		// prev_Kr_hat_.push_back(Kr_hat);
	}
	// tracking_error = pose_info - ym;

	//use boost ode solver to compute Ky_hat and Kr_hat:: lot more stable than trapezoidal rule
	runge_kutta_dopri5<state,double,state,double,vector_space_algebra> stepper;
	Ky_hat_dot = -Gamma_y * pose_info * tracking_error.transpose() * P * B  * sgnLambda;
	Kr_hat_dot = -Gamma_r * ref_      * tracking_error.transpose() * P * B  * sgnLambda;

	//use reference for the derivative
	stepper.do_step([](const state& x, state & dxdt, const double t)->void{
		dxdt = x;
	}, Ky_hat_dot, counter, Ky_hat, 0.01);
	//integrate Ky_hat_dot
	stepper.do_step([](const state& x, state & dxdt, const double t)->void{
		dxdt = x;
	}, Kr_hat_dot, counter, Kr_hat, 0.01);

	Eigen::VectorXd net_control;
	net_control.resize(m);
	if(update_net_law)	{
		std::lock_guard<std::mutex> net_pred_locker(pred_mutex);
		update_net_law = false;
		net_control = this->net_control;
	}
	/*
	* Calculate Control Law
	*/
	if(with_net_){
		u_control = (Ky_hat.transpose() * pose_info) +
					(Kr_hat.transpose() * ref_) + this->net_control;
	}
	else{
		u_control = (Ky_hat.transpose() * pose_info) +
					(Kr_hat.transpose() * ref_);
	}
	// u_control = this->net_control;
	/*
	Here are the rules that govern the bladders
	l_i --> Roll+	r_i -->Roll-
	l_o --> Roll-   r_o -->Roll+
	b_i --> Pitch+, Z+   b_o -->Pitch-, Z-
	u_{o+} is a suitable controller magnitude; u_+ is a +ve input
	------------------------------------------------------------
	DOF     |  Control Law
	------------------------------------------------------------
	Roll+   |  if f_{li} = u_{+}:
			|	f_{ri} = 0,
			|   f_{lo} = |u_{lo}|
			| 	f_{ro} = |u_{ro}|
	------------------------------------------------------------
	Roll-   | if f_{ri} = u_{+}:
			|  	f_{li} = 0;
			|   f_{ro} = 0  or u_{o+}
	------------------------------------------------------------
	Pitch+  | if f_{bi} = u_{+}
			|    f_{bo} = u_{o+} and f_{li} =f_{ri} = u_{head+}
	------------------------------------------------------------
	Pitch-  | f_{bo} = u_{max-}
			| f_{bi} = 0 or < f_{bo}
	------------------------------------------------------------
	Z+      | f_{li} = f_{ri} = u_{head+}
			| f_{bi} = u_{+} f_{bo} = u_{o+}
	------------------------------------------------------------
	Z-      | f_{bo} = u_{-}
			| f_{bi} = 0 or f_{bo}
	*/
	// // saturate control signals
	// for(auto i = 0; i < 6; ++i){
	// 	if(u_control[i] < 0)
	// 		u_control[i] = 0;
	// 	else if (u_control[i] > 1)
	// 		u_control[i] = 1;
	// 	else
	// 		u_control[i] = u_control[i];
	// }
	u_valves_.left_bladder_pos  = u_control(0);
	u_valves_.left_bladder_neg  = u_control(1);
	u_valves_.base_bladder_pos  = u_control(2);
	u_valves_.base_bladder_neg  = u_control(3);
	u_valves_.right_bladder_pos = u_control(4);
	u_valves_.right_bladder_neg = u_control(5);

	if(save) {
		std::ofstream file_handle;
		file_handle.open(ref_pose_file_, std::ofstream::out | std::ofstream::app);
		file_handle  << ref_(0) <<"\t" <<ref_(1) << "\t" << ref_(2) << "\t" << pose_info(0) <<"\t" <<pose_info(1) << "\t" << pose_info(2) << "\n";
		file_handle.close();
	}
	ros::Rate sleeper(2);
	sleeper.sleep();
	control_pub_.publish(u_valves_);
	vectorToHeadPose(std::move(pose_info), pose_);	// convert from eigen to headpose
	udp::sender s(io_service, boost::asio::ip::address::from_string(multicast_address), u_valves_, ref_, pose_);
	// pose is  [roll, z, pitch]
	// udp::sender s(io_service, boost::asio::ip::address::from_string(multicast_address), pose_); // used for identification

	if(print)	{
		OUT("\nref_: " 			<< ref_.transpose());
		OUT("y  (roll, z,  pitch): " 		 << pose_info.transpose());
		OUT("ym (roll, z,  pitch): " 		 << ym.transpose());
		OUT("e  (y-ym): " << tracking_error.transpose());
		OUT("pred (z, z, pitch, pitch, roll, roll): " << pred.transpose());
		OUT("net_control: " << net_control.transpose());
		OUT("Control Law: " << u_control.transpose());
		OUT("Kr_hat: \n" << Kr_hat);
		OUT("Ky_hat: \n" << Ky_hat);
	}
	++counter;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "controller_node", ros::init_options::AnonymousName);
	ros::NodeHandle n;
	bool print, useSigma, save, useVicon(true);

	Eigen::Vector3d ref;
	ref.resize(3);

	try{
		//supply values from the cmd line or retrieve them
		//from the ros parameter server
		n.getParam("/nn_controller/Reference/z", ref(1));    	//ref z
		n.getParam("/nn_controller/Reference/pitch", ref(2));	//ref pitch
		n.getParam("/nn_controller/Reference/roll", ref(0));	    //ref roll
		n.getParam("/nn_controller/Utils/print", print);
		n.getParam("/nn_controller/Utils/useSigma", useSigma);
		save = n.getParam("/nn_controller/Utils/save", save);
	}
	catch(std::exception& e){
		e.what();
	}

	Controller c(n, ref, print, useSigma, save);

	ros::Subscriber sub_pose = n.subscribe("/mannequine_head/pose", 100, &Controller::pose_subscriber, &c);
	ros::Subscriber sub_pred = n.subscribe("/mannequine_pred/preds", 100, &Controller::net_control_subscriber, &c);
	ros::spin();

	if(!ros::ok())
		ros::shutdown();

	return 0;
}


#include "solver.hpp"
#include <Eigen/Dense>
#ifdef SPECTRAL_VERBOSE
#	include <iostream>
#endif

namespace graphseg { namespace detail {

std::vector<EigenComponent> solver_eigen(const Eigen::MatrixXf& A, unsigned int num_ev)
{
	// solve eigensystem
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver;
	solver.compute(A);
#ifdef SPECTRAL_VERBOSE
	std::cout << "DEBUG: solver_eigen_dense_gev says " << solver.info() << std::endl;
#endif
	// collect eigenvectors and eigenvalues
	std::vector<EigenComponent> solution(std::min<std::size_t>(A.rows(), num_ev));
	for(unsigned int i=0; i<solution.size(); i++) {
		solution[i].eigenvalue = solver.eigenvalues()[i];
#ifdef SPECTRAL_VERBOSE
		std::cout << "DEBUG:\teigenvalue #" << i << "=" << solution[i].eigenvalue << std::endl;
#endif
		solution[i].eigenvector = solver.eigenvectors().col(i);
	}
	return solution;
}

}}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__CARE_HPP_
#define CBR_CONTROL__CARE_HPP_

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <type_traits>
#include <numeric>

namespace cbr
{

/* -------------------------------------------------------------------------- */
/*                               Matrix Balance                               */
/* -------------------------------------------------------------------------- */

// Numerical Recipes p.593 - Balancing Trasformation
// D^-1 * A * D = B

template<std::size_t nx, class T = double>
void matrix_balance(
  const Eigen::Ref<const Eigen::Matrix<T, 2 * nx, 2 * nx>> A,
  Eigen::Ref<Eigen::Matrix<T, 2 * nx, 2 * nx>> D,
  Eigen::Ref<Eigen::Matrix<T, 2 * nx, 2 * nx>> B
)
{
  std::size_t n = 2 * nx;

  // Initialize D and B
  D.setIdentity();
  B = A;

  double RADIX = 2.0;
  double sqrdx = RADIX * RADIX;

  std::size_t done = 0;

  while (done != 1) {
    done = 1;

    for (std::size_t i = 0; i < n; i++) {
      double r = 0.;
      double c = 0.;

      for (std::size_t j = 0; j < n; j++) {
        if (j != i) {
          c = c + std::abs(B(j, i));
          r = r + std::abs(B(i, j));
        }
      }

      if ((c != 0) && (r != 0)) {
        double g = r / RADIX;
        double f = 1.0;
        double s = c + r;

        while (c < g) {
          f = f * RADIX;
          c = c * sqrdx;
        }

        g = r * RADIX;

        while (c > g) {
          f = f / RADIX;
          c = c / sqrdx;
        }

        if (((c + r) / f) < 0.95 * s) {
          done = 0;
          g = 1 / f;
          D(i, i) = D(i, i) * f;

          for (std::size_t j = 0; j < n; j++) {
            B(i, j) = B(i, j) / f;
          }

          for (std::size_t j = 0; j < n; j++) {
            B(j, i) = B(j, i) * f;
          }
        }
      }
    }
  }
}

// Finds the solution to the ARE by finding the Eigen-decomposition of the Hamiltonean.
template<std::size_t nx, std::size_t nu, class T = double>
bool care(
  const Eigen::Ref<const Eigen::Matrix<T, nx, nx>> A,
  const Eigen::Ref<const Eigen::Matrix<T, nx, nu>> B,
  const Eigen::Ref<const Eigen::Matrix<T, nx, nx>> Q,
  const Eigen::Ref<const Eigen::Matrix<T, nu, nu>> R,
  Eigen::Ref<Eigen::Matrix<T, nx, nx>> P,
  Eigen::Ref<Eigen::Matrix<T, nu, nx>> K
)
{
// Ensure R positive definite (R>0)
  const Eigen::LLT<Eigen::Matrix<T, nu, nu>> Rdecomposed(R.transpose());
  if (Rdecomposed.info() == Eigen::NumericalIssue) {
    return false;
  }

  using H_t = Eigen::Matrix<T, 2 * nx, 2 * nx>;
  using Hc_t = Eigen::Matrix<std::complex<T>, 2 * nx, 2 * nx>;
  using Ac_t = Eigen::Matrix<std::complex<T>, nx, nx>;

//  1. Define Hamiltonean:
//
//                      [  A | -(B/R)*B' ]
//                H =   [ ---|-----------]
//                      [ -Q |    -A'    ]

  H_t H;
  H.template topLeftCorner<nx, nx>() = A;
  H.template topRightCorner<nx,
    nx>() = -Rdecomposed.solve(B.transpose()).transpose() * B.transpose();
  H.template bottomLeftCorner<nx, nx>() = -Q;
  H.template bottomRightCorner<nx, nx>() = -A.transpose();

  //  2. Balance the Hamiltonean
  H_t D;
  H_t Hb;
  matrix_balance<nx, T>(H, D, Hb);

  //  3. Solve the ARE through eigen decomposition of Hb
  // Start by obtaining eigenvalues and eigenvectors
  const Eigen::EigenSolver<H_t> es(Hb);
  auto V = es.eigenvectors();
  const auto & l = es.eigenvalues();
  V = D * V;

  // Idendify which eigenvalues are positive and which are negative
  std::array<int, 2 * nx> ord_L;
  std::fill(ord_L.begin(), ord_L.end(), 0);

  for (std::size_t k = 0; k < (2 * nx); k++) {
    if (std::real(l[k]) < 0) {
      ord_L[k] = -1;
    } else if (std::real(l[k]) > 0) {
      ord_L[k] = 1;
    }
  }

  // Sorting indices - place positive on left side, negative on right
  std::array<std::size_t, 2 * nx> ord_index;
  std::iota(ord_index.begin(), ord_index.end(), 0LU);
  std::sort(
    ord_index.begin(), ord_index.end(),
    [&](const std::size_t i1, const std::size_t i2) {
      const auto & l1 = ord_L[i1];
      const auto & l2 = ord_L[i2];
      return l1 > l2;
    });

  // Sort Eigenvector based on ord_index array
  Hc_t V_ord;
  for (std::size_t i = 0; i < 2 * nx; i++) {
    V_ord.col(i) = V.col(ord_index[i]);
  }

  // Define upper and lower right side block matrices
  const Ac_t V12 = V_ord.template topRightCorner<nx, nx>();
  const Ac_t V22 = V_ord.template bottomRightCorner<nx, nx>();

  const Eigen::FullPivLU<Ac_t> V12decomposed(V12.transpose());
  const Ac_t P_complex = V12decomposed.solve(V22.transpose()).transpose();  // P_complex = T22/T12

  // 4. Write Results
  P = P_complex.unaryExpr([](const std::complex<T> & v) {return std::real(v);});

  K = R.inverse() * B.transpose() * P;

  return true;
}

}    // namespace cbr

#endif  // CBR_CONTROL__CARE_HPP_


#ifndef _POSE_ESTIMATION_UKF_HPP
#define _POSE_ESTIMATION_UKF_HPP

#include <iostream>
#include <stdexcept>
#include <ukfom/ukf.hpp>
#include <ukfom/mtkwrap.hpp>
#include <boost/shared_ptr.hpp>
#include <base/Time.hpp>
#include <boost/noncopyable.hpp>

namespace pose_estimation
{

template<typename Manifold>
class UnscentedKalmanFilter : private boost::noncopyable
{
public:
    enum {
        DOF = Manifold::DOF
    };
    typedef Manifold State;
    typedef ukfom::mtkwrap<Manifold> WState;
    typedef ukfom::ukf<WState> MTK_UKF;
    typedef typename MTK_UKF::cov Covariance;

    UnscentedKalmanFilter()
    {
        process_noise_cov = Covariance::Zero();
        last_measurement_time.microseconds = 0;
        min_time_delta = 1.0e-9;
        max_time_delta = std::numeric_limits<double>::max();
    }

    virtual ~UnscentedKalmanFilter() {}

    /**
     * (Re-)initializes the UKF filter from a given state.
     */
    void initializeFilter(const State& initial_state, const Covariance& state_cov)
    {
        ukf.reset(new MTK_UKF(initial_state, state_cov));
        last_measurement_time.microseconds = 0;
    }

    /**
     * Provides the current state and covariance of the filter.
     *
     * @returns false if the filter has not been initialized.
     */
    bool getCurrentState(State& state, Covariance& state_cov) const
    {
        if(ukf.get() != NULL)
        {
            state = ukf->mu();
            state_cov = ukf->sigma();
            return true;
        }
        return false;
    }

    /**
     * Provides the current state of the filter.
     *
     * @returns false if the filter has not been initialized.
     */
    bool getCurrentState(State& state) const
    {
        if(ukf.get() != NULL)
        {
            state = ukf->mu();
            return true;
        }
        return false;
    }

    /**
     * Computes the time delta from a given sample timestamp and
     * calls predictionStep(delta_t)
     *
     * @throws runtime_error if delta_t is negative or greater then the allowed maximum.
     */
    void predictionStepFromSampleTime(const base::Time& sample_time)
    {
        // first call
        if(last_measurement_time.isNull())
        {
            last_measurement_time = sample_time;
            return;
        }

        // compute delta t
        double delta_t = (sample_time - last_measurement_time).toSeconds();

        // set new last measurement time
        if(delta_t > min_time_delta)
            last_measurement_time = sample_time;

        predictionStep(delta_t);
    }

    /**
     * Calls the predictionStepImpl after checking the delta_t value.
     *
     * @throws runtime_error if delta_t is negative or greater then the allowed maximum.
     */
    void predictionStep(double delta_t)
    {
        // check delta time
        if(delta_t < 0.0)
        {
            throw std::runtime_error("Delta time is negative!");
        }
        else if(delta_t <= min_time_delta)
        {
            // delta time is zero or close to zero
            return;
        }
        else if(delta_t > max_time_delta)
        {
            throw std::runtime_error("Delta time is greater then the allowed maximum!");
        }

        predictionStepImpl(delta_t);
    }

    unsigned getStateSize() const {return unsigned(WState::DOF);}
    bool isInitialized() const {return ukf.get() != NULL;}
    const Covariance& getProcessNoiseCovariance() const {return process_noise_cov;}
    void setProcessNoiseCovariance(const Covariance& noise_cov) {process_noise_cov = noise_cov;}
    const base::Time& getLastMeasurementTime() const {return last_measurement_time;}
    void setLastMeasurementTime(const base::Time& last_measurement_time)
                               {this->last_measurement_time = last_measurement_time;}
    double getMaxTimeDelta() const {return max_time_delta;}
    void setMaxTimeDelta(double max_time_delta) {this->max_time_delta = max_time_delta;}
    double getMinTimeDelta() const {return min_time_delta;}
    void setMinTimeDelta(double min_time_delta) {this->min_time_delta = min_time_delta;}

protected:
    virtual void predictionStepImpl(double delta_t) = 0;

    template<int DIM, typename scalar_type>
    void checkMeasurment(const Eigen::Matrix<scalar_type, DIM, 1>& mu, const Eigen::Matrix<scalar_type, DIM, DIM>& cov) const
    {
        if(!mu.allFinite() || !cov.allFinite())
            throw std::runtime_error("Measurement or covariance contains non-finite values!");
    }

protected:
    boost::shared_ptr<MTK_UKF> ukf;
    Covariance process_noise_cov;
    base::Time last_measurement_time;
    double max_time_delta;
    double min_time_delta;
};

}

#endif


//
// Copyright (c) 2015-2018 CNRS
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include "pinocchio/utils/timer.hpp"

#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)

int main(int argc, const char ** argv)
{
  using namespace Eigen;
  using namespace pinocchio;

  PinocchioTicToc timer(PinocchioTicToc::US);
  #ifdef NDEBUG
  const int NBT = 1000*100;
  #else
    const int NBT = 1;
    std::cout << "(the time score in debug mode is not relevant) " << std::endl;
  #endif

  pinocchio::Model model;

  std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
  if(argc>1) filename = argv[1];

  bool with_ff = true;
  if(argc>2)
  {
    const std::string ff_option = argv[2];
    if(ff_option == "-no-ff")
      with_ff = false;
  }

  if( filename == "HS")
    pinocchio::buildModels::humanoidRandom(model,true);
  else
    if(with_ff)
      pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
    else
      pinocchio::urdf::buildModel(filename,model);
  std::cout << "nq = " << model.nq << std::endl;

  pinocchio::Data data(model);
  VectorXd q = VectorXd::Random(model.nq);
  VectorXd qdot = VectorXd::Random(model.nv);
  VectorXd qddot = VectorXd::Random(model.nv);

  std::vector<VectorXd> qs     (NBT);
  std::vector<VectorXd> qdots  (NBT);
  std::vector<VectorXd> qddots (NBT);
  for(size_t i=0;i<NBT;++i)
    {
      qs[i]     = Eigen::VectorXd::Random(model.nq);
      qs[i].segment<4>(3) /= qs[i].segment<4>(3).norm();
      qdots[i]  = Eigen::VectorXd::Random(model.nv);
      qddots[i] = Eigen::VectorXd::Random(model.nv);
    }

  timer.tic();
  SMOOTH(NBT)
    {
      rnea(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
    }
  std::cout << "RNEA = \t\t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    nonLinearEffects(model,data,qs[_smooth],qdots[_smooth]);
  }
  std::cout << "NLE = \t\t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    rnea(model,data,qs[_smooth],qdots[_smooth],Eigen::VectorXd::Zero(model.nv));
  }
  std::cout << "NLE via RNEA = \t\t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
    {
      crba(model,data,qs[_smooth]);
    }
  std::cout << "CRBA = \t\t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    crbaMinimal(model,data,qs[_smooth]);
  }
  std::cout << "CRBA minimal = \t\t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
  }
  std::cout << "computeAllTerms = \t\t"; timer.toc(std::cout,NBT);

  double total = 0;
  SMOOTH(NBT)
    {
      crba(model,data,qs[_smooth]);
      timer.tic();
      cholesky::decompose(model,data);
      total += timer.toc(timer.DEFAULT_UNIT);
    }
  std::cout << "Branch Induced Sparsity Cholesky = \t" << (total/NBT)
	    << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;

  total = 0;
  Eigen::LDLT<Eigen::MatrixXd> Mldlt(data.M);
  SMOOTH(NBT)
  {
    crba(model,data,qs[_smooth]);
    data.M.triangularView<Eigen::StrictlyLower>()
    = data.M.transpose().triangularView<Eigen::StrictlyLower>();
    timer.tic();
    Mldlt.compute(data.M);
    total += timer.toc(timer.DEFAULT_UNIT);
  }
  std::cout << "Dense Eigen Cholesky = \t" << (total/NBT)
  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;

  timer.tic();
  SMOOTH(NBT)
    {
      computeJointJacobians(model,data,qs[_smooth]);
    }
  std::cout << "Jacobian = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    computeJointJacobiansTimeVariation(model,data,qs[_smooth],qdots[_smooth]);
  }
  std::cout << "Jacobian Time Variation = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
    {
      jacobianCenterOfMass(model,data,qs[_smooth],true);
    }
  std::cout << "COM+Jcom = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    centerOfMass(model,data,qs[_smooth], qdots[_smooth], qddots[_smooth], true);
  }
  std::cout << "COM+vCOM+aCOM = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    forwardKinematics(model,data,qs[_smooth]);
  }
  std::cout << "Zero Order Kinematics = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    forwardKinematics(model,data,qs[_smooth],qdots[_smooth]);
  }
  std::cout << "First Order Kinematics = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    forwardKinematics(model,data,qs[_smooth],qdots[_smooth], qddots[_smooth]);
  }
  std::cout << "Second Order Kinematics = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    ccrba(model,data,qs[_smooth],qdots[_smooth]);
  }
  std::cout << "CCRBA = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    aba(model,data,qs[_smooth],qdots[_smooth], qddots[_smooth]);
  }
  std::cout << "ABA = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    emptyForwardPass(model,data);
  }
  std::cout << "Empty Forward Pass = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    computeCoriolisMatrix(model,data,qs[_smooth],qdots[_smooth]);
  }
  std::cout << "Coriolis Matrix = \t"; timer.toc(std::cout,NBT);

  timer.tic();
  SMOOTH(NBT)
  {
    computeMinverse(model,data,qs[_smooth]);
  }
  std::cout << "Minv = \t"; timer.toc(std::cout,NBT);

  std::cout << "--" << std::endl;
  return 0;
}


#include <path_follower/supervisor/distancetopathsupervisor.h>
#include <path_msgs/FollowPathResult.h>
#include <paths.h>
#include <cslibs_navigation_utilities/Line2d.h>
#include <Eigen/Core>

using namespace Eigen;

namespace {
//! Module name, that is used for ros console output
const std::string MODULE = "s_disttopath";
}

DistanceToPathSupervisor::DistanceToPathSupervisor(double max_distance_to_path):
    max_dist_(max_distance_to_path),
    visualizer_(Visualizer::getInstance())
{}

void DistanceToPathSupervisor::supervise(Supervisor::State &state, Supervisor::Result *out)
{
    double dist = calculateDistanceToCurrentPathSegment(state);
    ROS_DEBUG_NAMED(MODULE, "Distance to current path segment: %g m", dist);
    if (dist > max_dist_) {
        //parent_.say("abort: too far away!"); //TODO: give supervisors access to say().

        ROS_WARN_NAMED(MODULE, "Moved too far away from the path (%g m, limit: %g m). Abort.",
                 dist, max_dist_);

        out->can_continue = false;
        out->status = path_msgs::FollowPathResult::RESULT_STATUS_PATH_LOST;
    }
}

double DistanceToPathSupervisor::calculateDistanceToCurrentPathSegment(const Supervisor::State &state)
{
    /* Calculate line from last way point to current way point (which should be the line the robot is driving on)
     * and calculate the distance of the robot to this line.
     */

    // Get previous waypoint
    // If the current waypoint is the first in this sub path (i.e. index == 0), use the next
    // instead. (I am not absolutly sure if this a good behaviour, so observe this via debug-output).
    int wp1_idx = 0;
    if (state.path->getWaypointIndex() > 0) {
        wp1_idx = state.path->getWaypointIndex() - 1;
    } else {
        // if wp_idx == 0, use the segment from 0th to 1st waypoint.
        wp1_idx = 1;

        ROS_DEBUG_NAMED(MODULE, "Toggle waypoints as wp_idx == 0 in calculateDistanceToCurrentPathSegment() (%s, line %d)", __FILE__, __LINE__);
    }

    geometry_msgs::Pose wp1 = state.path->getWaypoint(wp1_idx);
    geometry_msgs::Pose wp2 = state.path->getCurrentWaypoint();

    // line from last waypoint to current one.
    Line2d segment_line(Vector2d(wp1.position.x, wp1.position.y), Vector2d(wp2.position.x, wp2.position.y));

    ///// visualize start and end point of the current segment (for debugging)
    visualizer_->drawMark(24, wp1.position, "segment_marker", 0, 1, 1);
    visualizer_->drawMark(25, wp2.position, "segment_marker", 1, 0, 1);
    /////

    // get distance of robot (slam_pose_) to segment_line.
    return segment_line.GetDistance(state.robot_pose.head<2>());
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/matrix/adjugate.hpp>
#include <fcppt/math/matrix/comparison.hpp>
#include <fcppt/math/matrix/object_impl.hpp>
#include <fcppt/math/matrix/output.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <ostream>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_adjugate
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		int,
		3,
		3
	>
	matrix_type;

	matrix_type const t(
		fcppt::math::matrix::row(
			-3, 2, -5
		),
		fcppt::math::matrix::row(
			-1, 0, -2
		),
		fcppt::math::matrix::row(
			3, -4, 1
		)
	);

	matrix_type const result(
		fcppt::math::matrix::adjugate(
			t
		)
	);

	std::cout
		<< result
		<< '\n';

	BOOST_CHECK_EQUAL(
		result,
		matrix_type(
			fcppt::math::matrix::row(
				-8,18,-4
			),
			fcppt::math::matrix::row(
				-5,12,-1
			),
			fcppt::math::matrix::row(
				4,-6,2
			)
		)
	);
}


#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include "simple_cycles.hpp"
#include <boost/test/unit_test.hpp>

using namespace bglex;

BOOST_AUTO_TEST_CASE(test1) {
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS>
      Graph;
  Graph G(6);
  boost::add_edge(0, 1, G);
  boost::add_edge(1, 2, G);
  boost::add_edge(2, 3, G);
  boost::add_edge(3, 1, G);
  boost::add_edge(2, 4, G);
  boost::add_edge(4, 5, G);
  boost::add_edge(5, 3, G);

  vector<vector<int>> reference = {{1, 2, 4, 5, 3}, {1, 2, 3}};

  vector<vector<Graph::vertex_descriptor>> cycles;
  cycles = bglex::simple_cycles<Graph>(G);

  BOOST_CHECK_EQUAL(cycles.size(), reference.size());
  for (auto n = 0; n < cycles.size(); n++) {
    BOOST_CHECK_EQUAL(cycles[n].size(), reference[n].size());
    for (auto m = 0; m < cycles[n].size(); m++) {
      BOOST_CHECK_EQUAL(cycles[n][m], reference[n][m]);
    }
  }
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, fma_double) {
  using stan::math::fma;
  EXPECT_FLOAT_EQ(1.0, fma(3.0, 2.0, -5));
  EXPECT_FLOAT_EQ(0.0, fma(2.0, 3.0, -6));
  EXPECT_FLOAT_EQ(46.9, fma(4.5, 2.0, 37.9));
}

TEST(MathFunctions, fma_int) {
  using stan::math::fma;
  EXPECT_FLOAT_EQ(11.0, fma(int(3),int(2),int(5)));
}

TEST(MathFunctions, fma_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::fma(nan, 3.0, 2.7));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::fma(3.0, nan, 1.5));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::fma(2, -8.2, nan));
}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl; using namespace std;
    typedef  mtl::dense2D<double>            Matrix;

    double array[][4]= {{2, 3,   4,   5},
                        {4, 10, 13,  16},
                        {6, 25, 38,  46},
		        {8, 32, 77, 100}};
    Matrix 		A(array), I(4, 4);
    I= 1.0;

    Matrix LU(A);
    dense_vector<std::size_t> v(4);
    lu(LU, v);
    mat::traits::permutation<>::type P(permutation(v));

    cout << "A is:\n" << A << "\nPermuted A is \n" << Matrix(P * A);

    Matrix L(I + strict_lower(LU)), U(upper(LU)), A2(L * U);
    cout << "L [permuted] is:\n" << L << "U [permuted] is:\n" << U
	 << "L * U [permuted] is:\n" << A2
	 << "L * U is:\n" << Matrix(trans(P) * A2);

    Matrix UI(inv_upper(U));
    cout << "inv(U) [permuted] is:\n" << UI << "UI * U is:\n" << UI * U;

    Matrix LI(inv_lower(L));
    cout << "inv(L) [permuted] is:\n" << LI << "LI * L is:\n" << LI * L;

    Matrix AI(UI * LI * P);
    cout << "inv(A) [inv(U) * inv(L) * P] is \n" << AI << "Test: A * AI is\n" << AI * A;

    mat::traits::inv<Matrix>::type A_inv(inv(A));
    cout << "inv(A) is \n" << A_inv << "Test: A * AI is\n" << A_inv * A;

    return 0;
}


//==================================================================================================
/*!
  @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_ACOTH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOTH_HPP_INCLUDED

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

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic cotangent argument \f$\frac12\log\frac{x+1}{x-1}\f$

    @see cosh, sinh, acosh, asinh, atanh, asech, acosh, acsch

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

    @par Example:

      @snippet acoth.cpp acoth

    @par Possible output:

      @snippet acoth.txt acoth

  **/
  IEEEValue acoth(IEEEValue const & x);
} }
#endif

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

#endif


//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// 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)
//-----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax operator
#include <boost/test/unit_test.hpp>

#include <mitrax/operator.hpp>
#include <mitrax/compare.hpp>

using boost::typeindex::type_id;
using boost::typeindex::type_id_runtime;
using namespace mitrax;
using namespace mitrax::literals;

template < typename T >
auto rt_id(T&& v){
	return type_id_runtime(static_cast< T&& >(v));
}

template < typename T >
auto const id = type_id< T >();

constexpr auto ref1 = make_matrix< int >(3_DS, {
	{0, 1, 2},
	{3, 4, 5},
	{6, 7, 8}
});

constexpr auto ref2 = make_matrix< int >(3_DS, {
	{0, 2, 4},
	{6, 8, 10},
	{12, 14, 16}
});

constexpr auto ref3 = make_matrix< int >(3_DS, {
	{0, 3, 6},
	{9, 12, 15},
	{18, 21, 24}
});

constexpr auto ref1_mod3 = make_matrix< int >(3_DS, {
	{0, 1, 2},
	{0, 1, 2},
	{0, 1, 2}
});

constexpr auto ref1_square = make_matrix< int >(3_DS, {
	{0, 1, 4},
	{9, 16, 25},
	{36, 49, 64}
});

constexpr auto ref1_element_plus1 = make_matrix< int >(3_DS, {
	{1, 2, 3},
	{4, 5, 6},
	{7, 8, 9}
});

constexpr auto ref1_element_minus1 = make_matrix< int >(3_DS, {
	{-1, 0, 1},
	{2, 3, 4},
	{5, 6, 7}
});

constexpr auto ref_all1 = make_matrix< int >(3_DS, {
	{1, 1, 1},
	{1, 1, 1},
	{1, 1, 1}
});

constexpr auto ref_all3 = make_matrix< int >(3_DS, {
	{3, 3, 3},
	{3, 3, 3},
	{3, 3, 3}
});

constexpr auto ref_all9 = make_matrix< int >(3_DS, {
	{9, 9, 9},
	{9, 9, 9},
	{9, 9, 9}
});

constexpr auto ref_mod = make_matrix< int >(3_DS, {
	{2, 3, 4},
	{5, 6, 7},
	{8, 9, 10}
});

constexpr auto ref_all9_mod_res = make_matrix< int >(3_DS, {
	{1, 0, 1},
	{4, 3, 2},
	{1, 0, 9}
});

// TODO: also check non compile time versions
// TODO: also check non square versions

BOOST_AUTO_TEST_SUITE(suite_operator)

BOOST_AUTO_TEST_CASE(test_element_plus_assign){
	auto m = ref1;

	element_plus_assign(m, 1);
	auto eq = m == ref1_element_plus1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_minus_assign){
	auto m = ref1;

	element_minus_assign(m, 1);
	auto eq = m == ref1_element_minus1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_multiplies_assign){
	auto m = ref1;

	m *= 3;
	auto eq = m == ref3;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_divides_assign){
	auto m = ref3;

	m /= 3;
	auto eq = m == ref1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_modulus_assign){
	auto m = ref1;

	m %= 3;
	auto eq = m == ref1_mod3;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_plus_assign){
	auto m = ref1;

	m += ref1;
	auto eq = m == ref2;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_minus_assign){
	auto m = ref2;

	m -= ref1;
	auto eq = m == ref1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_multiplies_assign){
	auto m = ref1;

	element_multiplies_assign(m, ref1);
	auto eq = m == ref1_square;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_divides_assign){
	auto m = ref1_element_plus1;

	element_divides_assign(m, ref1_element_plus1);
	auto eq = m == ref_all1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_modulus_assign){
	auto m = ref_all9;

	element_modulus_assign(m, ref_mod);
	auto eq = m == ref_all9_mod_res;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_plus){
	auto m = ref1;

	auto res = element_plus(m, 1);
	auto eq = res == ref1_element_plus1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_minus){
	auto m = ref1;

	auto res = element_minus(m, 1);
	auto eq = res == ref1_element_minus1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_multiplies){
	auto m = ref1;

	auto res1 = m * 3;
	auto res2 = 3 * m;
	auto eq1 = res1 == ref3;
	auto eq2 = res2 == ref3;
	BOOST_TEST(eq1);
	BOOST_TEST(eq2);
}

BOOST_AUTO_TEST_CASE(test_divides){
	auto m = ref3;

	auto res = m / 3;
	auto eq = res == ref1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_modulus){
	auto m = ref1;

	auto res = m % 3;
	auto eq = res == ref1_mod3;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_plus){
	auto m = ref1;

	auto res = m + ref1;
	auto eq = res == ref2;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_minus){
	auto m = ref2;

	auto res = m - ref1;
	auto eq = res == ref1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_multiplies){
	auto m = ref1;

	auto res = element_multiplies(m, ref1);
	auto eq = res == ref1_square;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_divides){
	auto m = ref1_element_plus1;

	auto res = element_divides(m, ref1_element_plus1);
	auto eq = res == ref_all1;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_element_modulus){
	auto m = ref_all9;

	auto res = element_modulus(m, ref_mod);
	auto eq = res == ref_all9_mod_res;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_matrix_multiplies){
	auto m = ref_all1 * ref_all1;

	auto eq = m == ref_all3;
	BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(test_transpose){
	constexpr auto m_cc = make_matrix< int >(2_CS, 3_RS, {
		{1, 2},
		{3, 4},
		{5, 6}
	});

	auto m_CD = make_matrix< int >(2_CS, 3_RD, {
		{1, 2},
		{3, 4},
		{5, 6}
	});

	auto m_dc = make_matrix< int >(2_CD, 3_RS, {
		{1, 2},
		{3, 4},
		{5, 6}
	});

	auto m_DD = make_matrix< int >(2_CD, 3_RD, {
		{1, 2},
		{3, 4},
		{5, 6}
	});

	/*constexpr*/ auto trans_cc = transpose(m_cc); //TODO:need constexpr lambda
	auto trans_CD = transpose(m_CD);
	auto trans_dc = transpose(m_dc);
	auto trans_DD = transpose(m_DD);

	auto check1 = [](auto const& m){
		return
			m.cols() == 3_CS &&
			m.rows() == 2_RS &&
			m(0_c, 0_r) == 1 &&
			m(1_c, 0_r) == 3 &&
			m(2_c, 0_r) == 5 &&
			m(0_c, 1_r) == 2 &&
			m(1_c, 1_r) == 4 &&
			m(2_c, 1_r) == 6;
	};

	auto check2 = [](auto const& m){
		return
			m.cols() == 2_CS &&
			m.rows() == 3_RS &&
			m(0_c, 0_r) == 1 &&
			m(1_c, 0_r) == 2 &&
			m(0_c, 1_r) == 3 &&
			m(1_c, 1_r) == 4 &&
			m(0_c, 2_r) == 5 &&
			m(1_c, 2_r) == 6;
	};

	BOOST_TEST(check1(trans_cc));
	BOOST_TEST(check1(trans_CD));
	BOOST_TEST(check1(trans_dc));
	BOOST_TEST(check1(trans_DD));

	BOOST_TEST((rt_id(trans_cc) == id< std_matrix< int, 3_C, 2_R > >));
	BOOST_TEST((rt_id(trans_CD) == id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(trans_dc) == id< std_matrix< int, 3_C, 0_R > >));
	BOOST_TEST((rt_id(trans_DD) == id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check2(transpose(trans_cc)));
	BOOST_TEST(check2(transpose(trans_CD)));
	BOOST_TEST(check2(transpose(trans_dc)));
	BOOST_TEST(check2(transpose(trans_DD)));
}

BOOST_AUTO_TEST_CASE(test_unary_plus){
	auto m = ref1_element_minus1;

	auto n = +m;

	BOOST_TEST((
			n.cols() == 3_CS &&
			n.rows() == 3_RS &&
			n(0_c, 0_r) == -1 &&
			n(1_c, 0_r) == 0 &&
			n(2_c, 0_r) == 1 &&
			n(0_c, 1_r) == 2 &&
			n(1_c, 1_r) == 3 &&
			n(2_c, 1_r) == 4 &&
			n(0_c, 2_r) == 5 &&
			n(1_c, 2_r) == 6 &&
			n(2_c, 2_r) == 7
	));
}

BOOST_AUTO_TEST_CASE(test_unary_minus){
	auto m = ref1_element_minus1;

	auto n = -m;

	BOOST_TEST((
			n.cols() == 3_CS &&
			n.rows() == 3_RS &&
			n(0_c, 0_r) == 1 &&
			n(1_c, 0_r) == 0 &&
			n(2_c, 0_r) == -1 &&
			n(0_c, 1_r) == -2 &&
			n(1_c, 1_r) == -3 &&
			n(2_c, 1_r) == -4 &&
			n(0_c, 2_r) == -5 &&
			n(1_c, 2_r) == -6 &&
			n(2_c, 2_r) == -7
	));
}

BOOST_AUTO_TEST_CASE(test_abs){
	auto m = ref1_element_minus1;

	auto n = abs(m);

	BOOST_TEST((
			n.cols() == 3_CS &&
			n.rows() == 3_RS &&
			n(0_c, 0_r) == 1 &&
			n(1_c, 0_r) == 0 &&
			n(2_c, 0_r) == 1 &&
			n(0_c, 1_r) == 2 &&
			n(1_c, 1_r) == 3 &&
			n(2_c, 1_r) == 4 &&
			n(0_c, 2_r) == 5 &&
			n(1_c, 2_r) == 6 &&
			n(2_c, 2_r) == 7
	));
}

BOOST_AUTO_TEST_SUITE_END()


// #define BOOST_TEST_DYN_LINK
// #define BOOST_TEST_MODULE Suites
// #include <boost/test/unit_test.hpp>

#include<iostream>
#include "basis.hpp"
#include "operators.hpp"
#include"numerics.hpp"
#include"diag.hpp"
#include "files.hpp"
// using namespace boost::unit_test;
// using boost::unit_test_framework::test_suite;
// using namespace Many_Body;
// BOOST_AUTO_TEST_SUITE(timeevesting)
// BOOST_AUTO_TEST_CASE(timeev)
using namespace Many_Body;
using namespace Eigen;
  template <typename Matrix>
    double diag_with_lancz(Eigen::VectorXcd& initialState, const Matrix& ham, const Matrix& obs, size_t lanczosDim)
  { double sum=0;

    Eigen::VectorXcd initialState2=initialState;
    Eigen::MatrixXcd Q(ham.rows(), lanczosDim);
    Many_Body::TriDiagMat tri=Many_Body::Lanczos(ham, initialState2, lanczosDim, Q);
    Eigen::MatrixXd S(Q.cols(), Q.cols());
              Eigen::VectorXd eigenVals(Q.cols());
    Many_Body::diag(tri, S, eigenVals);
   Eigen::MatrixXcd S2=S.cast<std::complex<double>>();
   VectorXcd initialStateTemp(Q.cols());
    initialStateTemp= (S2.row(0).transpose());
   //  // initialStateTemp= S2*initialStateTemp;
   //  // initialState= Q*initialStateTemp;
   //  assert(std::abs(initialState.norm() -1.) < Many_Body::err);
     Eigen::MatrixXcd obs2=S.adjoint()*Q.adjoint()*obs*Q*S;
    for (size_t i = 0; i < lanczosDim; ++i)
      {

      std::complex<double> c=std::abs(initialStateTemp(i))*std::abs(initialStateTemp(i))*obs2(i, i);
    	 sum+=real(c);
    }
    return sum;

  }
int main()
{

    size_t numberOfSteps=5;
   const size_t L=8;
   ElectronBasis<L> e(4);
   size_t D=700;
   //std::cout << e << std::endl;
   Eigen::MatrixXcd AA = Eigen::MatrixXcd::Random(D, D);
   	 Eigen::MatrixXcd H = AA + AA.adjoint();
   //  Operators::Mat H= Operators::NumberOperator(e)+Operators::EKinOperator(e);
   // Eigen::VectorXcd inistate(e.D);

   // inistate.setZero();
   // inistate[0]=1;
	 Eigen::VectorXcd inistate=Eigen::VectorXcd::Random(D);
	 inistate=inistate/inistate.norm();
   Eigen::VectorXd eigenVals(D);
   Eigen::MatrixXcd BB = Eigen::MatrixXcd::Random(D, D);
   	 Eigen::MatrixXcd O = BB + BB.adjoint();
   // Operators::Mat O(e.dim, e.dim);
   //  O.setZero();
   //  O.coeffRef(1, 1)=0.5;
   //  O.coeffRef(2, 2)=0.5;

    // HAmiltonian =H
   Eigen::MatrixXcd HH=Eigen::MatrixXcd(H);
   // Eigen::MatrixXd HH2=Eigen::MatrixXd(H);

   Many_Body::diag(HH, eigenVals);
Eigen::VectorXcd newIn=inistate;
Eigen::VectorXcd newIn2=inistate;
  double evalL=diag_with_lancz(newIn2, H, O, 20);
 double evalN=0;
  MatrixXcd M=HH.adjoint()*O*HH;
  newIn=HH.adjoint()*newIn;
  for(size_t i=0; i<D; i++)
   {

        std::complex<double> c=M(i, i)*std::abs(newIn(i))*std::abs(newIn2(i));
        evalN+=real(c);
   }
   std::cout<< evalN << "  " << evalL << "  " << D <<std::endl;
  return 0;
}

//BOOST_AUTO_TEST_SUITE_END()
// EOF


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

  Program:   Visualization Toolkit
  Module:    vtkLaplacianInfilling.cxx
  Author: Pierre Guilbert

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

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

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

// LOCAL
#include "vtkLaplacianInfilling.h"

// STD
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>

// VTK
#include <vtkObjectFactory.h>
#include <vtkImageData.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkXMLImageDataWriter.h>

// BOOST
#include <boost/algorithm/string.hpp>

// Eigen
#include <Eigen/Sparse>

// Implementation of the New function
vtkStandardNewMacro(vtkLaplacianInfilling)

//-----------------------------------------------------------------------------
int vtkLaplacianInfilling::RequestData(vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector, vtkInformationVector *outputVector)
{
  // Get the input
  vtkImageData * inputImage = vtkImageData::GetData(inputVector[0]->GetInformationObject(0));

  // Get the output
  vtkImageData* outputImage = vtkImageData::GetData(outputVector->GetInformationObject(0));
  outputImage->ShallowCopy(inputImage);

  int xBound = outputImage->GetDimensions()[0];
  int yBound = outputImage->GetDimensions()[1];
  int nParams = xBound * yBound;

  Eigen::SparseMatrix<double> Laplacian(nParams, nParams);
  Eigen::VectorXd Y(nParams); // The values of the laplacian required

  // Triplet of value: row, column and value
  std::vector<Eigen::Triplet<double> > nonZeroCoefficient;
  for (int x = 0; x < xBound; ++x)
  {
    for (int y = 0; y < yBound; ++y)
    {
      int flattenIndex = x + xBound * y;

      // check if the current pixel has a value
      double value = inputImage->GetScalarComponentAsDouble(x, y, 0, 0);
      if ((std::abs(value) > std::numeric_limits<double>::epsilon()))
      {
        // we don't want this value to be modified
        // contraint: xi = yi
        nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex, 1.0));
        Y(flattenIndex) = value;
      }
      else
      {
        // else fill it solving the laplace equation
        // using finite difference scheme and Dirichlet
        // boundary
        int validNeigh = 0;

        // Laplacian constraints matrix:
        // Neighbors contraint
        if (x != 0)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex - 1, 1));
          validNeigh++;
        }
        if (x != xBound - 1)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex + 1, 1));
          validNeigh++;
        }
        if (y != 0)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex - xBound, 1));
          validNeigh++;
        }
        if (y != yBound - 1)
        {
          nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex + xBound, 1));
          validNeigh++;
        }
        // Diagonal constraint
        nonZeroCoefficient.push_back(Eigen::Triplet<double>(flattenIndex, flattenIndex, -1.0 * static_cast<double>(validNeigh)));
        // We want the laplacian to be null
        Y(flattenIndex) = 0;
      }
    }
  }

  // Fill Laplacian constraints matrix
  Laplacian.setFromTriplets(nonZeroCoefficient.begin(), nonZeroCoefficient.end());

  // Solving:
  Eigen::SparseLU< Eigen::SparseMatrix<double> > solver(Laplacian);
  Eigen::MatrixXd X = solver.solve(Y);

  // X contains the Dirichlet solution function
  // values i.e: 0-values pixel are filled with
  // laplacian
  for (int x = 0; x < xBound; ++x)
  {
    for (int y = 0; y < yBound; ++y)
    {
      int flattendIndex = x + xBound * y;
      outputImage->SetScalarComponentFromDouble(x, y, 0, 0, X(flattendIndex));
    }
  }

  return 1;
}


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

// Written by Cornelius Steinhardt

#include <cmath>

// #include <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename Matrix>
void test1(Matrix& m, double tau)
{
  mtl::mat::inserter<Matrix> ins(m);
  size_t nrows=num_rows(m);
  double val;
  for (size_t r=0;r<nrows;++r)
  {
    for (size_t c=0;c<nrows;++c)
    {
      if(r==c)
        ins(r,c) << 1.;
      else
      {
        val=2.*(static_cast<double>(rand())/RAND_MAX - 0.5);
        if (val<tau)
          ins(r,c) << val;
      }
    }
  }
}

int main(int, char**)
{

  const int N = 2; // Original from Jan had 2000
  const int Niter = 5000;
  using itl::pc::identity; using itl::pc::ilu_0; using itl::pc::ic_0; using itl::pc::diagonal;
  //typedef mtl::dense2D<double> matrix_type;
  //typedef compressed2D<std::complex<double>mat::parameters<tag::col_major> > matrix_type;
  typedef mtl::compressed2D<double> matrix_type;
  matrix_type                   A(N, N);
  laplacian_setup(A, N, N);
  mtl::dense_vector<double> b(N*N, 1), x(N*N),r(N*N);
  identity<matrix_type>         Ident(A);
  //ic_0<matrix_type>             ic(A);
  //ilu_0<matrix_type>            ilu(A);
  //diagonal<matrix_type>         diag(A);

  //test1(A,0.194);
  std::cout << "A has " << A.nnz() << " non-zero entries" << std::endl;
  std::cout << "A =\n"  << A << " \n";

  std::cout << "Non- preconditioned bicgstab  Won't convergence (for large examples)!" << std::endl;
  x= 2.0;
  itl::basic_iteration<double> iter_0(b, Niter, 1.e-8);

  bicgstab(A, x, b, Ident, iter_0);
  r= A*x-b;

  std::cout << "START GMRES  Won't convergence (for large examples)!" << std::endl;
  std::cout << "\n Non-preconditioned gmres(1)" << std::endl;
  x= 5.0;
  itl::basic_iteration<double> iter_1(b, 1, 1.e-8);
  gmres(A, x, b, Ident, Ident, iter_1, 1);
  r= A*x-b;
  if (two_norm(r) > 0.00005) throw "gmres doesn't converge";

  std::cout << "\n Non-preconditioned gmres(2) (doesn't converge even for the test)" << std::endl;
  x= 2.0;
  itl::basic_iteration<double> iter_2(b, 8, 1.e-8);
  gmres(A, x, b, Ident, Ident, iter_2, 2);
  r= A*x-b;
  // if (two_norm(r) > 0.00005) throw "gmres(2) doesn't converge";
  if (two_norm(r) > 0.00005) std::cout << "GMRES(2) didn't converge after 8 titerations.\n";

#if 1
  std::cout << "\n Non-preconditioned gmres(4)" << std::endl;
  x= 2.5;
  itl::basic_iteration<double> iter_4(b, 16, 1.e-8);
  gmres(A, x, b, Ident, Ident,  iter_4, 4);
  r= A*x-b;
  // if (two_norm(r) > 0.000001) throw "gmres(4) doesn't converge even with more iterations and restarts";
  if (two_norm(r) > 0.00005) std::cout << "GMRES(4) didn't converge after 16 titerations.\n";

  std::cout << "\n Non-preconditioned gmres(4) more iterations " << std::endl;
  x= 2.5;
  itl::basic_iteration<double> iter_5(b, 32, 1.e-8);
  gmres(A, x, b, Ident, Ident,  iter_5, 4);
  r= A*x-b;
  // if (two_norm(r) > 0.00005) throw "gmres(4) doesn't converge";

  std::cout << "\n Non-preconditioned gmres(8)" << std::endl;
  x= 2.5;
  itl::basic_iteration<double> iter_8(b, 8, 1.e-8);
  gmres(A, x, b, Ident, Ident, iter_8, 8);
  r= A*x-b;
  if (two_norm(r) > 0.00005) throw "gmres(8) doesn't converge";

  std::cout << "\n Non-preconditioned gmres(16)" << std::endl;
  x= 2.5;
  itl::basic_iteration<double> iter_16(b, 32, 1.e-8);
  gmres(A, x, b, Ident, Ident, iter_16, 16);
  r= A*x-b;
  if (two_norm(r) > 0.00005) throw "gmres(16) doesn't converge";

  std::cout << "\n Non-preconditioned gmres(32)" << std::endl;
  x= 2.5;
  itl::basic_iteration<double> iter_32(b, 32, 1.e-8);
  gmres(A, x, b, Ident, Ident, iter_32, 32);
  r= A*x-b;
  if (two_norm(r) > 0.00005) throw "gmres(32) doesn't converge";
#endif

  test1(A,0.194);
  std::cout << "A has " << A.nnz() << " non-zero entries" << std::endl;
  std::cout << "A =\n"  << A << " \n";

  return 0;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/Gaussian.h>
#include <BayesFilters/GaussianFilter.h>
#include <BayesFilters/sigma_point.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/UKFCorrection.h>
#include <BayesFilters/UKFPrediction.h>
#include <BayesFilters/utils.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>

#include <string>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class UKFSimulation : public GaussianFilter
{
public:
    UKFSimulation
    (
        Gaussian& initial_state,
        std::unique_ptr<GaussianPrediction> prediction,
        std::unique_ptr<GaussianCorrection> correction,
        std::size_t simulation_steps
    ) noexcept :
        GaussianFilter(std::move(prediction), std::move(correction)),
        predicted_state_(initial_state.dim_linear, initial_state.dim_circular),
        corrected_state_(initial_state),
        simulation_steps_(simulation_steps)
    { }

protected:
    bool run_condition() override
    {
        if (step_number() < simulation_steps_)
            return true;
        else
            return false;
    }

    bool initialization_step() override
    {
        return true;
    }

    void filtering_step() override
    {
        prediction().predict(corrected_state_, predicted_state_);
        correction().freeze_measurements();
        correction().correct(predicted_state_, corrected_state_);

        log();
    }

    std::vector<std::string> log_file_names(const std::string& folder_path, const std::string& file_name_prefix) override
    {
        return {folder_path + "/" + file_name_prefix + "_pred_mean",
                folder_path + "/" + file_name_prefix + "_cor_mean"};
    }

    void log() override
    {
        logger(predicted_state_.mean().transpose(), corrected_state_.mean().transpose());
    }

private:
    Gaussian predicted_state_;

    Gaussian corrected_state_;

    std::size_t simulation_steps_;
};

int main(int argc, char* argv[])
{
    std::cout << "Running a UKF filter on a simulated target." << std::endl;

    const bool write_to_file = (argc > 1 ? std::string(argv[1]) == "ON" : false);
    if (write_to_file)
        std::cout << "Data is logged in the test folder with prefix testUKF." << std::endl;

    /* A set of parameters needed to run an unscented Kalman filter in a simulated environment. */
    Vector4d initial_simulated_state(10.0f, 0.0f, 10.0f, 0.0f);
    std::size_t simulation_time = 100;
    /* Initialize unscented transform parameters. */
    double alpha = 1.0;
    double beta = 2.0;
    double kappa = 0.0;

    /* Step 1 - Initialization */

    std::size_t state_size = 4;
    Gaussian initial_state(state_size);
    Vector4d initial_mean(4.0f, 0.04f, 15.0f, 0.4f);
    Matrix4d initial_covariance;
    initial_covariance << pow(0.05, 2), 0,            0,            0,
                          0,            pow(0.05, 2), 0,            0,
                          0,            0,            pow(0.01, 2), 0,
                          0,            0,            0,            pow(0.01, 2);
    initial_state.mean() = initial_mean;
    initial_state.covariance() = initial_covariance;

    /* Step 2 - Prediction */

    /* Step 2.1 - Define the state model. */

    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<AdditiveStateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(WhiteNoiseAcceleration::Dim::TwoD, T, tilde_q);

    /* Step 2.2 - Define the prediction step. */

    /* Initialize the unscented Kalman filter prediction step and pass the ownership of the state model */
    std::unique_ptr<UKFPrediction> ukf_prediction = utils::make_unique<UKFPrediction>(std::move(wna), state_size, alpha, beta, kappa);

    /* Step 3 - Correction */

    /* Step 3.1 - Define where the measurement are originated from (simulated in this case). */

    /* Initialize simulated target model with a white noise acceleration. */
    std::unique_ptr<AdditiveStateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(WhiteNoiseAcceleration::Dim::TwoD, T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_simulated_state, simulation_time);

    if (write_to_file)
        simulated_state_model->enable_log("./", "testUKF");

    /* Step 3.2 - Initialize a measurement model (a linear sensor reading x and y coordinates). */
    double sigma_x = 10.0;
    double sigma_y = 10.0;
    Eigen::MatrixXd R(2, 2);
    R << std::pow(sigma_x, 2.0),                    0.0,
                            0.0, std::pow(sigma_y, 2.0);

    std::unique_ptr<AdditiveMeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model), SimulatedLinearSensor::LinearMatrixComponent{ 4, std::vector<std::size_t>{ 0, 2 } }, R);

    if (write_to_file)
        simulated_linear_sensor->enable_log("./", "testUKF");

    /* Step 3.3 - Initialize the unscented Kalman filter correction step and pass the ownership of the measurement model. */
    std::unique_ptr<UKFCorrection> ukf_correction = utils::make_unique<UKFCorrection>(std::move(simulated_linear_sensor), state_size, alpha, beta, kappa);

    /* Step 4 - Assemble the unscented Kalman filter. */
    std::cout << "Constructing unscented Kalman filter..." << std::flush;

    UKFSimulation ukf(initial_state, std::move(ukf_prediction), std::move(ukf_correction), simulation_time);

    if (write_to_file)
        ukf.enable_log("./", "testUKF");

    std::cout << "done!" << std::endl;

    /* Step 5 - Boot the filter. */
    std::cout << "Booting unscented Kalman filter..." << std::flush;

    ukf.boot();

    std::cout << "completed!" << std::endl;

    /* Step 6 - Run the filter and wait until it is closed. */
    /* Note that since this is a simulation, the filter will end upon simulation termination. */
    std::cout << "Running unscented Kalman filter..." << std::flush;

    ukf.run();

    std::cout << "waiting..." << std::flush;

    if (!ukf.wait())
        return EXIT_FAILURE;

    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


/*
 * Copyright (C) 2019 by AutoSense Organization. All rights reserved.
 * Gary Chan <chenshj35@mail2.sysu.edu.cn>
 */

#ifndef COMMON_INCLUDE_COMMON_BOUNDING_BOX_HPP_
#define COMMON_INCLUDE_COMMON_BOUNDING_BOX_HPP_

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <boost/numeric/ublas/matrix.hpp>

#include <boost/geometry/geometries/adapted/c_array.hpp>
BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

#include "common/common.hpp"
#include "common/types/object.hpp"

namespace autosense {
namespace common {
namespace bbox {
/*
 *                           |x
 *(x_max,y_max) ---width-----|
 *      |                    |
 *      |                  length
 *      |                    |
 *  y<-----------------(x_min,y_min)
 */
typedef struct {
    double x_max;  // left-top corner
    double y_max;
    double x_min;  // right-bottom corner
    double y_min;
} BoundingBox;

// Orientation Bounding Box
typedef struct {
    BoundingBox box;
    double yaw_rad;
} OBB2;

// Orientation Bounding Box
typedef struct {
    double gc_x, gc_y, gc_z;
    double yaw_rad;
    double h, w, l;
} GroundBox;

typedef boost::geometry::model::polygon<
    boost::geometry::model::d2::point_xy<double>>
    Polygon;

/**
 * @brief Object's 3D OBB to 2D ground box
 * @param object
 * @param gbox
 */
static void toGroundBox(ObjectConstPtr object, GroundBox* gbox) {
    gbox->gc_x = object->ground_center(0);
    gbox->gc_y = object->ground_center(1);
    gbox->gc_z = object->ground_center(2);
    gbox->yaw_rad = object->yaw_rad;
    gbox->h = object->height;
    gbox->w = object->width;
    gbox->l = object->length;
}

static void toGroundBox(const Eigen::Vector3f& center,
                        const Eigen::Vector3f& size,
                        const double& yaw,
                        GroundBox* gbox) {
    gbox->gc_x = center(0);
    gbox->gc_y = center(1);
    gbox->gc_z = center(2);
    gbox->yaw_rad = yaw;
    gbox->h = size(0);
    gbox->w = size(1);
    gbox->l = size(2);
}

/**
 * @brief Intersection-over-Union
 */
static double bbIoU(const BoundingBox& box1, const BoundingBox& box2) {
    double box1_length = box1.x_max - box1.x_min;
    double box1_width = box1.y_max - box1.y_min;
    double area1 = box1_length * box1_width;

    double box2_length = box2.x_max - box2.x_min;
    double box2_width = box2.y_max - box2.y_min;
    double area2 = box2_length * box2_width;

    if (box1.x_min > box2.x_max) {
        return 0.0;
    }
    if (box1.y_min > box2.y_max) {
        return 0.0;
    }
    if (box1.x_max < box2.x_min) {
        return 0.0;
    }
    if (box1.y_max < box2.y_min) {
        return 0.0;
    }
    double inter_x =
        std::min(box1.x_max, box2.x_max) - std::max(box1.x_min, box2.x_min);
    double inter_y =
        std::min(box1.x_max, box2.x_max) - std::max(box1.x_min, box2.x_min);
    double intersection = inter_x * inter_y;
    return intersection / (area1 + area2 - intersection);
}

/*
 * @brief compute polygon of an oriented bounding box
 * @note Apollo's Object Coordinate
 *          |x
 *      C   |   D-----------
 *          |              |
 *  y---------------     length
 *          |              |
 *      B   |   A-----------
 */
template <typename T>
Polygon toPolygon(const T& g) {
    using boost::numeric::ublas::matrix;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(g.yaw_rad);
    mref(0, 1) = -sin(g.yaw_rad);
    mref(1, 0) = sin(g.yaw_rad);
    mref(1, 1) = cos(g.yaw_rad);

    matrix<double> corners(2, 4);
    // -------------(l/2,w/2)(l/2,-w/2)(-l/2,-w/2)(-l/2,w/2)
    double data[] = {g.l / 2, g.l / 2,  -g.l / 2, -g.l / 2,
                     g.w / 2, -g.w / 2, -g.w / 2, g.w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = boost::numeric::ublas::prod(mref, corners);

    for (int i = 0; i < 4; ++i) {
        gc(0, i) += g.gc_x;
        gc(1, i) += g.gc_y;
    }

    double points[][2] = {{gc(0, 0), gc(1, 0)},
                          {gc(0, 1), gc(1, 1)},
                          {gc(0, 2), gc(1, 2)},
                          {gc(0, 3), gc(1, 3)},
                          {gc(0, 0), gc(1, 0)}};
    Polygon poly;
    boost::geometry::append(poly, points);
    return poly;
}

/**
 * @brief Intersection-over-Union
 */
static double groundBoxIoU(const GroundBox& box1, const GroundBox& box2) {
    Polygon gp = toPolygon(box1);
    Polygon dp = toPolygon(box2);

    std::vector<Polygon> in, un;
    boost::geometry::intersection(gp, dp, in);
    boost::geometry::union_(gp, dp, un);

    double inter_area = in.empty() ? 0. : boost::geometry::area(in.front());
    double union_area = boost::geometry::area(un.front());

    double o = 0.;
    // union
    o = inter_area / union_area;
    // bbox_a
    // o = inter_area / area(dp);
    // bbox_b
    // o = inter_area / area(gp);
    return o;
}

static bool groundBoxInside(const GroundBox& box1, const GroundBox& box2) {
    Polygon gp = toPolygon(box1);
    Polygon dp = toPolygon(box2);

    std::vector<Polygon> in;
    boost::geometry::intersection(gp, dp, in);
    if (in.empty()) {
        return false;
    } else {
        double inter_area = boost::geometry::area(in.front());
        double box1_area = boost::geometry::area(gp);
        return abs(box1_area - inter_area) < EPSILON;
    }
}

/**
 * @brief check box1 is overlapping with box2
 *  true: box1 is inside box2 or box2 is inside box1
 *  true: IoU between box1 and box2 > threshold_IoU
 * @param box1
 * @param box2
 * @param threshold_IoU
 * @return
 */
static bool groundBoxOverlap(const GroundBox& box1,
                             const GroundBox& box2,
                             double threshold_IoU) {
    if (groundBoxInside(box1, box2) || groundBoxInside(box2, box1)) {
        return true;
    }

    if (groundBoxIoU(box1, box2) > threshold_IoU) {
        return true;
    }

    return false;
}

/**
 * @brief predict object size and ground center based on object cloud and
 * previous direction
 * @tparam PointT
 * @param cloud
 * @param direction
 * @param size
 * @param center
 */
template <typename PointCloudPtrT>
void computeBboxSizeCenter(PointCloudPtrT cloud,
                           const Eigen::Vector3d& direction,
                           Eigen::Vector3d* size,
                           Eigen::Vector3d* center) {
    Eigen::Vector3d dir(direction[0], direction[1], 0);
    dir.normalize();
    Eigen::Vector3d ortho_dir(-dir[1], dir[0], 0.0);

    Eigen::Vector3d z_dir(dir.cross(ortho_dir));

    Eigen::Vector3d min_pt(DBL_MAX, DBL_MAX, DBL_MAX);
    Eigen::Vector3d max_pt(-DBL_MAX, -DBL_MAX, -DBL_MAX);
    Eigen::Vector3d loc_pt;
    for (size_t i = 0u; i < cloud->size(); ++i) {
        Eigen::Vector3d pt = Eigen::Vector3d(
            cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
        loc_pt[0] = pt.dot(dir);
        loc_pt[1] = pt.dot(ortho_dir);
        loc_pt[2] = pt.dot(z_dir);
        for (size_t j = 0u; j < 3; ++j) {
            min_pt[j] = std::min(min_pt[j], loc_pt[j]);
            max_pt[j] = std::max(max_pt[j], loc_pt[j]);
        }
    }

    *size = max_pt - min_pt;
    *center = dir * ((max_pt[0] + min_pt[0]) * 0.5) +
              ortho_dir * ((max_pt[1] + min_pt[1]) * 0.5) + z_dir * min_pt[2];
}

}  // namespace bbox
}  // namespace common
}  // namespace autosense

#endif  // COMMON_INCLUDE_COMMON_BOUNDING_BOX_HPP_


/*
 libs/numeric/odeint/examples/stochastic_euler.hpp

 Copyright 2012 Karsten Ahnert
 Copyright 2012 Mario Mulansky

 Stochastic euler stepper example and Ornstein-Uhlenbeck process

 Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <vector>
#include <iostream>
#include <boost/random.hpp>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

/*
//[ stochastic_euler_class_definition
template< size_t N > class stochastic_euler
{
public:

    typedef boost::array< double , N > state_type;
    typedef boost::array< double , N > deriv_type;
    typedef double value_type;
    typedef double time_type;
    typedef unsigned short order_type;
    typedef boost::numeric::odeint::stepper_tag stepper_category;

    static order_type order( void ) { return 1; }

    // ...
};
//]
*/

/*
//[ stochastic_euler_do_step
template< size_t N > class stochastic_euler
{
public:

    // ...

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt ) const
    {
        deriv_type det , stoch ;
        system.first( x , det );
        system.second( x , stoch );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] += dt * det[i] + sqrt( dt ) * stoch[i];
    }
};
//]
*/

//[ stochastic_euler_class
template< size_t N >
class stochastic_euler
{
public:

    typedef boost::array< double , N > state_type;
    typedef boost::array< double , N > deriv_type;
    typedef double value_type;
    typedef double time_type;
    typedef unsigned short order_type;

    typedef boost::numeric::odeint::stepper_tag stepper_category;

    static order_type order( void ) { return 1; }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt ) const
    {
        deriv_type det , stoch ;
        system.first( x , det );
        system.second( x , stoch );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] += dt * det[i] + sqrt( dt ) * stoch[i];
    }
};
//]

//[ stochastic_euler_ornstein_uhlenbeck_def
const static size_t N = 1;
typedef boost::array< double , N > state_type;

struct ornstein_det
{
    void operator()( const state_type &x , state_type &dxdt ) const
    {
        dxdt[0] = -x[0];
    }
};

struct ornstein_stoch
{
    boost::mt19937 m_rng;
    boost::normal_distribution<> m_dist;

    ornstein_stoch( double sigma ) : m_rng() , m_dist( 0.0 , sigma ) { }

    void operator()( const state_type &x , state_type &dxdt )
    {
        dxdt[0] = m_dist( m_rng );
    }
};
//]

struct streaming_observer
{
    template< class State >
    void operator()( const State &x , double t ) const
    {
        std::cout << t << "\t" << x[0] << "\n";
    }
};

int main( int argc , char **argv )
{
    using namespace std;
    using namespace boost::numeric::odeint;

    //[ ornstein_uhlenbeck_main
    double dt = 0.1;
    state_type x = {{ 1.0 }};
    integrate_const( stochastic_euler< N >() , make_pair( ornstein_det() , ornstein_stoch( 1.0 ) ) ,
            x , 0.0 , 10.0 , dt , streaming_observer() );
    //]
    return 0;
}


// Copyright (c) 2015 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef I23DSFM_GEOMETRY_HALF_SPACE_HPP_
#define I23DSFM_GEOMETRY_HALF_SPACE_HPP_

#include "i23dSFM/linearProgramming/linearProgrammingOSI_X.hpp"
#include <Eigen/Geometry>
#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Hyperplane<double,3>)

namespace i23dSFM {
namespace geometry {
namespace halfPlane {

/// Define the Half_plane equation (abcd coefficients)
typedef Eigen::Hyperplane<double,3> Half_plane;
/// Define a collection of Half_plane
typedef std::vector<Half_plane> Half_planes;

// Define a plane passing through the points (p, q, r).
// The plane is oriented such that p, q and r are oriented in a positive sense (that is counterclockwise).
static Half_plane Half_plane_p(const Vec3 & p, const Vec3 & q, const Vec3 & r)
{
  const Vec3 abc = (p-r).cross(q-r);
  const double d = - abc.dot(r);
  Half_plane hp;
  hp.coeffs() << abc(0),abc(1),abc(2),d;
  return hp;
}

// [1] Paper: Finding the intersection of n half-spaces in time O(n log n).
// Author: F.P. Preparata, D.E. Muller
// Published in: Theoretical Computer Science, Volume 8, Issue 1, Pages 45-55
// Year: 1979
// More: ISSN 0304-3975, http://dx.doi.org/10.1016/0304-3975(79)90055-0.

/// Return true if the half_planes define a not empty volume (an intersection exists)
static bool isNotEmpty(const Half_planes & hplanes)
{
  // Check if it exists a point on all positive side of the half plane thanks to a Linear Program formulation [1].
  // => If a point exists: there is a common subspace defined and so intersections.
  // The LP formulation consists in set the Half_plane as constraint and check if a point can fit the equations.

  using namespace i23dSFM;
  using namespace i23dSFM::linearProgramming;

  LP_Constraints cstraint;
  {
    cstraint._nbParams = 3; // {X,Y,Z}
    cstraint._vec_bounds.resize(cstraint._nbParams);
    std::fill(cstraint._vec_bounds.begin(),cstraint._vec_bounds.end(),
      std::make_pair((double)-1e+30, (double)1e+30)); // [X,Y,Z] => -inf, +inf
    cstraint._bminimize = true;

    // Configure constraints
    const size_t nbConstraints = hplanes.size();
    cstraint._constraintMat = Mat(nbConstraints,3);
    cstraint._vec_sign.resize(nbConstraints);
    cstraint._Cst_objective = Vec(nbConstraints);

    // Fill the constrains (half-space equations)
    for (unsigned char i= 0; i < hplanes.size(); ++i)
    {
      const Vec & half_plane_coeff = hplanes[i].coeffs();
      // add the half plane equation to the system
      cstraint._constraintMat.row(i) =
        Vec3(half_plane_coeff(0),
          half_plane_coeff(1),
          half_plane_coeff(2));
      cstraint._vec_sign[i] = LP_Constraints::LP_GREATER_OR_EQUAL;
      cstraint._Cst_objective(i) = - half_plane_coeff(3);
    }
  }

  // Solve in order to see if a point exists within the half spaces positive side?
  OSI_CLP_SolverWrapper solver(cstraint._nbParams);
  solver.setup(cstraint);
  const bool bIntersect = solver.solve(); // Status of the solver tell if there is an intersection or not
  return bIntersect;
}

} // namespace geometry
} // namespace i23dSFM
} // namespace halfPlane

#endif // I23DSFM_GEOMETRY_HALF_SPACE_HPP_


#pragma once

#include <Eigen/Core>

#include "../../../../util/Maybe.hh"

#include "../../../Bounds.hh"
#include "../linesegment2.hh"

namespace bold
{
  class Line;

  struct LineSegment2i : public LineSegment2<int>
  {
  public:
    LineSegment2i(Eigen::Matrix<int, 2, 1> const& p1, Eigen::Matrix<int, 2, 1> const& p2)
    : LineSegment2<int>::LineSegment2(p1, p2)
    {}

    LineSegment2i(int x1, int y1, int x2, int y2)
    : LineSegment2<int>::LineSegment2(Point(x1, y1), Point(x2, y2))
    {}

    using LineSegment2<int>::LineSegment2;
    using LineSegment2<int>::operator=;

    double gradient() const;

    double yIntersection() const;

    /** Returns the angle of this line to the +ve x-azis, in the range [-pi, pi] */
    double angle() const;

    Maybe<LineSegment2i> cropTo(Bounds2i const& bounds) const;
  };
}


/// @file bealab/core/prelim/math.hpp
/// A number of math functions for real and complex numbers.

#ifndef _BEALAB_PRELIM_MATH_
#define	_BEALAB_PRELIM_MATH_

#include <cmath>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/math/special_functions/binomial.hpp>

namespace bealab
{
/// @defgroup prelim_math Math functions
/// A number of math functions, most of them imported from STD.
/// - Classification
///   - isnan
///   - isinf
///   - isfinite
/// - Basic functions
///   - min
///   - max
///   - round
///   - trunc
///   - ceil
///   - floor
///   - mod
/// - Complex functions
///   - abs
///   - arg
///   - real
///   - imag
///   - conj
///   - polar
/// - Trigonometric functions
///   - sin
///   - cos
///   - tan
///   - asin
///   - acos
///   - atan
/// - Hyperbolic functions
///   - sinh
///   - cosh
///   - tanh
///   - asinh
///   - acosh
///   - atanh
/// - Exponential and power functions
///   - exp
///   - log
///   - log2
///   - log10
///   - pow
///   - sqrt
/// - Special functions
///   - sinc
///   - factorial
///   - erf
///   - erfc
///   - erf_inv
///   - erfc_inv
/// @{

/// @name Classification
using std::isnan;
using std::isinf;
using std::isfinite;
/// @}

/// @name Basic functions
using std::min;
using std::max;
#ifdef BEALAB_MACOSX
using ::round;
using ::trunc;
#else
using std::round;
using std::trunc;
#endif
using std::ceil;
using std::floor;

/// x modulo y
inline double mod( double x, double y ) { return x - floor( x / y ) * y; }
/// @}

/// @name Complex functions
using std::abs;
using std::arg;
using std::real;
using std::imag;
using std::conj;
using std::polar;
//inline double abs( const complex& x )  { return std::abs(_complex(x)); }
//inline double arg( const complex& x ) { return std::arg(_complex(x)); }
//inline double real( const complex& x ) { return std::real(_complex(x)); }
//inline double imag( const complex& x ) { return std::imag(_complex(x)); }
//inline complex conj( const complex& x ) { return std::conj(_complex(x)); }
//inline complex polar( double r, double theta=0 ) { return std::polar(r,theta); }
/// @}

/// @name Trigonometric functions
using std::sin;
using std::cos;
using std::tan;
using std::asin;
using std::acos;
using std::atan;
//inline complex sin( const complex& x ) { return std::sin(_complex(x)); }
//inline complex cos( const complex& x ) { return std::cos(_complex(x)); }
//inline complex tan( const complex& x ) { return std::tan(_complex(x)); }
//inline complex asin( const complex& x ) { return std::asin(_complex(x)); }
//inline complex acos( const complex& x ) { return std::acos(_complex(x)); }
//inline complex atan( const complex& x ) { return std::atan(_complex(x)); }
/// @}

/// @name Hyperbolic functions
using std::sinh;
using std::cosh;
using std::tanh;
using std::asinh;
using std::acosh;
using std::atanh;
//inline complex sinh( const complex& x ) { return std::sinh(_complex(x)); }
//inline complex cosh( const complex& x ) { return std::cosh(_complex(x)); }
//inline complex tanh( const complex& x ) { return std::tanh(_complex(x)); }
//inline complex asinh( const complex& x ) { return std::asinh(_complex(x)); }
//inline complex acosh( const complex& x ) { return std::acosh(_complex(x)); }
//inline complex atanh( const complex& x ) { return std::atanh(_complex(x)); }
/// @}

/// @name Exponential and power functions
using std::exp;
using std::log;
#ifdef BEALAB_MACOSX
using ::log2;
#else
using std::log2;
#endif
using std::log10;
//inline complex exp( const complex& x ) { return std::exp(_complex(x)); }
//inline complex log( const complex& x ) { return std::log(_complex(x)); }
//inline complex log2( const complex& x ) { return log(_complex(x))/log(2); }
//inline complex log10( const complex& x ) { return std::log10(_complex(x)); }
using std::pow;
//inline double pow( double x, int y ) { return std::pow( x, y ); }
//inline complex pow( double x, double y )  { return std::pow( _complex(x), y ); }
//inline complex pow( const complex& x, int y ) { return std::pow(_complex(x),y); }
//inline complex pow( const complex& x, double y ) { return std::pow(_complex(x),y); }
//inline complex pow( double x, const complex& y ) { return std::pow(x,_complex(y)); }
using std::sqrt;
//inline complex sqrt( const complex& x ) { return std::sqrt(_complex(x)); }
/// @}

/// @name Special functions

/// Sine cardinal function
//inline double sinc( double x ) { return x == 0 ? 1 : sin( pi * x ) / (pi * x); }
inline double sinc( double x ) { return boost::math::sinc_pi( pi * x ); }

/// Factorial
inline double factorial( int i ) { return boost::math::factorial<double>(i); }

/// Binomial coefficients
inline double binomial_coefficient( int n, int k )
	{ return boost::math::binomial_coefficient<double>(n,k); }

using boost::math::erf;
using boost::math::erfc;
using boost::math::erf_inv;
using boost::math::erfc_inv;

/// @}

/// @}
}
#endif


#include "difference_of_squares.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(squares_of_sums)
{
    BOOST_REQUIRE_EQUAL(225, squares::square_of_sums(5));
    BOOST_REQUIRE_EQUAL(3025, squares::square_of_sums(10));
    BOOST_REQUIRE_EQUAL(25502500, squares::square_of_sums(100));
}

BOOST_AUTO_TEST_CASE(sum_of_squares)
{
    BOOST_REQUIRE_EQUAL(55, squares::sum_of_squares(5));
    BOOST_REQUIRE_EQUAL(385, squares::sum_of_squares(10));
    BOOST_REQUIRE_EQUAL(338350, squares::sum_of_squares(100));
}

BOOST_AUTO_TEST_CASE(up_to_5)
{
    BOOST_REQUIRE_EQUAL(225, squares::square_of_sums(5));
    BOOST_REQUIRE_EQUAL(55, squares::sum_of_squares(5));
    BOOST_REQUIRE_EQUAL(170, squares::difference(5));
}

BOOST_AUTO_TEST_CASE(up_to_10)
{
    BOOST_REQUIRE_EQUAL(3025, squares::square_of_sums(10));
    BOOST_REQUIRE_EQUAL(385, squares::sum_of_squares(10));
    BOOST_REQUIRE_EQUAL(2640, squares::difference(10));
}

BOOST_AUTO_TEST_CASE(up_to_100)
{
    BOOST_REQUIRE_EQUAL(25502500, squares::square_of_sums(100));
    BOOST_REQUIRE_EQUAL(338350, squares::sum_of_squares(100));
    BOOST_REQUIRE_EQUAL(25164150, squares::difference(100));
}
#if defined(EXERCISM_RUN_ALL_TESTS)
#endif


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2018 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Eigen <-> CImg data type conversions                                     *
***************************************************************************/

#ifndef EIGENCIMG_HPP
#define EIGENCIMG_HPP

#define cimg_display 0
#include <CImg.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>
#include <stdexcept>

#define __STR_FUNCNAME__  std::string(__FUNCTION__)

using namespace cimg_library;
using namespace Eigen;

class EigenCImg
{
public:
    template <typename T>
    static Matrix<T, Dynamic, Dynamic> toEigen(const CImg<T> &image);
    template <typename T>
    static Matrix<T, Dynamic, Dynamic> toEigen(const CImg<T> &image, const CImg<bool> &mask);
    template <class Derived>
    static CImg<typename Derived::Scalar> toCImg(const DenseBase<Derived> &data, const CImg<bool> &mask);
    template<typename T, int Dimensions>
    static CImg<T> toCImg(const Tensor<T, Dimensions> &data, const CImg<bool> &mask);
};

template <typename T>
Matrix<T, Dynamic, Dynamic> EigenCImg::toEigen(const CImg<T> &image)
{
    int i = 0;
    Matrix<T, Dynamic, Dynamic> data(image.width() * image.height() * image.depth(), image.spectrum());
    for (int x = 0; x < image.width(); ++x)
    {
        for (int y = 0; y < image.height(); ++y)
        {
            for (int z = 0; z < image.depth(); ++z)
            {
                for (int c = 0; c < image.spectrum(); ++c)
                    data(i, c) = (T) image(x, y, z, c);
                ++i;
            }
        }
    }
    return data;
}

template <typename T>
Matrix<T, Dynamic, Dynamic> EigenCImg::toEigen(const CImg<T> &image, const CImg<bool> &mask)
{
    if (image.height() != mask.height() || image.width() != mask.width() || image.depth() != mask.depth())
    {
        std::stringstream s;
        s << "In function " << __STR_FUNCNAME__ << " ==> Image and mask dimensions must agree." << std::endl;
        throw std::runtime_error(s.str());
    }

    int i = 0;
    Matrix<T, Dynamic, Dynamic> data(mask.size(), image.spectrum());
    for (int x = 0; x < image.width(); ++x)
    {
        for (int y = 0; y < image.height(); ++y)
        {
            for (int z = 0; z < image.depth(); ++z)
            {
                if (!mask(x, y, z))
                    continue;

                for (int c = 0; c < image.spectrum(); ++c)
                    data(i, c) = (T) image(x, y, z, c);
                ++i;
            }
        }
    }
    data.conservativeResize(i, data.cols());
    return data;
}

template <class Derived>
CImg<typename Derived::Scalar> EigenCImg::toCImg(const DenseBase<Derived> &data, const CImg<bool> &mask)
{
    if (mask.sum() != data.rows())
    {
        std::stringstream s;
        s << "In function " << __STR_FUNCNAME__ << " ==> Number of positive mask elements must agree with data rows." << std::endl;
        throw std::runtime_error(s.str());
    }

    int i = 0;
    CImg<typename Derived::Scalar> image(mask.width(), mask.height(), mask.depth(), data.cols());
    for (int x = 0; x < image.width(); ++x)
    {
        for (int y = 0; y < image.height(); ++y)
        {
            for (int z = 0; z < image.depth(); ++z)
            {
                if (!mask(x, y, z))
                {
                    for (int c = 0; c < data.cols(); ++c)
                        image(x, y, z, c) = (typename Derived::Scalar) 0;
                }
                else
                {
                    for (int c = 0; c < data.cols(); ++c)
                        image(x, y, z, c) = (typename Derived::Scalar) data(i, c);
                    ++i;
                }
            }
        }
    }
    return image;
}

template<typename T, int Dimensions>
CImg<T> EigenCImg::toCImg(const Tensor<T, Dimensions> &data, const CImg<bool> &mask)
{
    if (mask.sum() != (int) data.dimension(0))
    {
        std::stringstream s;
        s << "In function " << __STR_FUNCNAME__ << " ==> Number of positive mask elements must agree with data rows." << std::endl;
        throw std::runtime_error(s.str());
    }

    const int K = data.dimension(Dimensions - 1);

    typename Eigen::Tensor<T, Dimensions-2>::Dimensions dims;
    for (int i = 0; i < Dimensions - 2; ++i)
        dims[i] = i + 1;

    Eigen::Tensor<T, 2> dataReduced = data.sum(dims);

    int i = 0;
    CImg<T> image(mask.width(), mask.height(), mask.depth(), K);
    for (int x = 0; x < image.width(); ++x)
    {
        for (int y = 0; y < image.height(); ++y)
        {
            for (int z = 0; z < image.depth(); ++z)
            {
                if (!mask(x, y, z))
                {
                    for (int c = 0; c < K; ++c)
                        image(x, y, z, c) = (T) 0;
                }
                else
                {
                    for (int c = 0; c < K; ++c)
                        image(x, y, z, c) = (T) dataReduced(i, c);
                    ++i;
                }
            }
        }
    }
    return image;
}

#endif


#include <Eigen/Dense>
#include <stdio.h>
#include <math.h>
#include <omp.h>
#include "mvnormal.h"
extern "C" {
#include <sparse.h>
}

extern "C" void dsyrk_(char *uplo, char *trans, int *m, int *n, double *alpha, double a[],
            int *lda, double *beta, double c[], int *ldc);

using namespace Eigen;

void hello(double* x, double* y, int n, int k) {
  //cblas_dsyrk(CblasColMajor, CblasLower, CblasTrans, n, k, 1.0, x, k, 0.0, y, n);
  char lower  = 'L';
  char trans  = 'T';
  double one  = 1.0;
  double zero = 0.0;
  dsyrk_(&lower, &trans, &n, &k, &one, x, &k, &zero, y, &n);
}

void eigenQR(double* x, int nrow, int ncol) {
  MatrixXd X = Map<MatrixXd>(x, nrow, ncol);
  HouseholderQR<MatrixXd> qr(X);
  MatrixXd Q = qr.householderQ();
  printf("Q(0,0) = %f\n", Q(0,0));
}

MatrixXd getx() {
  init_bmrng(100099102);
  MatrixXd x(1000,10);
  bmrandn(x);
  return x;
}

/** x is [n x k] matrix
 *  y is [n x n] matrix
 *  x and y are column-ordered
 *  computes y = x * x'
 *  (storing only lower triangular part)
 */
void hello2(double* x, double* y, int n, int k) {
  if (n >= 256) {
    // probably broken
    //cblas_dsyrk(CblasColMajor, CblasLower, CblasNoTrans, n, k, 1.0, x, k, 0.0, y, n);
    char lower  = 'L';
    char trans  = 'N';
    double one  = 1.0;
    double zero = 0.0;
    dsyrk_(&lower, &trans, &n, &k, &one, x, &n, &zero, y, &n);
    return;
  }
  int nthreads = -1;
#pragma omp parallel
  {
#pragma omp single
    {
      nthreads = omp_get_num_threads();
    }
  }
  std::vector<MatrixXd> Ys;
  Ys.resize(nthreads, MatrixXd(n, n));

#pragma omp parallel
  {
    const int ithread  = omp_get_thread_num();
    int rows_per_thread = (int) 8 * ceil(k / 8.0 / nthreads);
    int row_start = rows_per_thread * ithread;
    int row_end   = rows_per_thread * (ithread + 1);
    if (row_end > k) {
      row_end = k;
    }
    double* xi = & x[ row_start * n ];
    int nrows  = row_end - row_start;
    MatrixXd X = Map<MatrixXd>(xi, n, nrows);
    MatrixXd & Y = Ys[ithread];
    Y.triangularView<Eigen::Lower>() = X * X.transpose();
  }
  for (int i = 0; i < n; i++) {
    for (int j = i; j < n; j++) {
      double tmp = 0;
      for (int k = 0; k < nthreads; k++) {
        tmp += Ys[k](j, i);
      }
      y[i*n + j] = tmp;
    }
  }
}

void At_mul_A_eig(Eigen::MatrixXd & A, Eigen::MatrixXd & C) {
  const int n = A.cols();
  if (n != C.rows()) { printf("A.cols() must equal C.rows()."); exit(1); }
  if (C.rows() != C.cols()) { printf("C.rows() must equal C.cols()."); exit(1); }
  C.triangularView<Eigen::Lower>() = A.transpose() * A;
}


#include <glm/models/links/power.hpp>

#include <armadillo>

using namespace arma;

power_link::power_link(float lambda)
    : glm_link::glm_link( "power" )
{
    m_lambda = lambda;
    if( lambda < 1e-6 )
    {
        m_lambda = 0.0;
    }
    else if( lambda > 2.0 )
    {
        m_lambda = 2.0;
    }
}

vec
power_link::init_beta(const mat &X, const vec &y) const
{
    return 0;
}

vec
power_link::mu(const arma::vec &eta) const
{
    if( m_lambda == 0.0 )
    {
        return log( eta );
    }
    else if( m_lambda == 2.0 )
    {
        return exp( eta );
    }
    else if( m_lambda < 1.0 )
    {
        return ( pow( eta, m_lambda ) - 1 ) / m_lambda;
    }
    else
    {
        return pow( 1 + eta*(2-m_lambda), 1/(2-m_lambda) );
    }
}

vec
power_link::eta(const arma::vec &mu) const
{
    if( m_lambda == 0.0 )
    {
        return exp( mu );
    }
    else if( m_lambda == 2.0 )
    {
        return log( mu );
    }
    else if( m_lambda < 1.0 )
    {
        return pow( 1 + mu*m_lambda, 1/m_lambda);
    }
    else
    {
        return (pow( mu, 2 - m_lambda ) - 1) / (2 - m_lambda);
    }
}

vec
power_link::mu_eta(const arma::vec &mu) const
{
    if( m_lambda == 0.0 )
    {
        return exp( mu );
    }
    else if( m_lambda == 2.0 )
    {
        return 1 / mu;
    }
    else if( m_lambda < 1.0 )
    {
        return pow( 1 + mu * m_lambda, 1/m_lambda - 1 );
    }
    else
    {
        return pow( mu, 1 - m_lambda );
    }
}


#ifndef PYBNESIAN_UTIL_CHISQUARESUM_HPP
#define PYBNESIAN_UTIL_CHISQUARESUM_HPP

#include <Eigen/Dense>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <util/rpoly.hpp>
#include <util/uniroot.hpp>

using boost::math::binomial_coefficient, boost::math::gamma_distribution, boost::math::cdf, boost::math::complement;
using Eigen::VectorXd, Eigen::Dynamic, Eigen::Matrix;

namespace util {

namespace detail {

template <typename VectorType>
VectorType chisquaresum_moments(VectorType& coeffs, int p) {
    using Scalar = typename VectorType::Scalar;
    VectorType cumulants(2 * p);

    cumulants(0) = coeffs.sum();
    cumulants(1) = 2 * coeffs.squaredNorm();

    // Start loop in r = 3, so 2^(r-1)*(r-1)! = 8
    Scalar fact_const = 8;
    for (int i = 2, end = 2 * p; i < end; ++i) {
        cumulants(i) = fact_const * coeffs.array().pow(i + 1).sum();
        fact_const *= 2 * (i + 1);
    }

    VectorType moments = cumulants;
    moments(1) += moments(0) * moments(0);
    for (int i = 2, end = 2 * p; i < end; ++i) {
        auto offset = cumulants(0) * moments(i - 1) + i * cumulants(1) * moments(i - 2);

        for (int j = 2; j < i; ++j) {
            offset += binomial_coefficient<Scalar>(i, j) * cumulants(j) * moments(i - j - 1);
        }

        moments(i) += offset;
    }

    return moments;
}

template <typename VectorType>
Matrix<typename VectorType::Scalar, Dynamic, Dynamic> delta_matrix_template(VectorType& moments, int size_matrix) {
    using Scalar = typename VectorType::Scalar;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;

    MatrixType t(size_matrix, size_matrix);

    t(0) = 1;
    t(0, 1) = t(1, 0) = moments(0);

    // Fill two first columns first
    for (int i = 2; i < size_matrix; ++i) {
        t(i, 0) = moments(i - 1);
    }

    for (int i = 1; i < size_matrix; ++i) {
        t(i, 1) = moments(i);
    }

    // Fill remaining columns
    for (int j = 2; j < size_matrix; ++j) {
        for (int i = 0; i < size_matrix; ++i) {
            t(i, j) = moments(i + j - 1);
        }
    }

    return t;
}

template <typename Scalar>
Matrix<Scalar, Dynamic, 1> delta_mult_coefficients(Scalar alpha, int size_matrix) {
    using VectorType = Matrix<Scalar, Dynamic, 1>;
    auto max_r = 2 * size_matrix - 2;

    VectorType mult_coefficients(max_r - 1);
    mult_coefficients(0) = 1 + alpha;
    for (int i = 1, end = max_r - 1; i < end; ++i) {
        mult_coefficients(i) = mult_coefficients(i - 1) * (1 + (i + 1) * alpha);
    }

    return mult_coefficients.cwiseInverse();
}

template <typename MatrixType>
void delta_apply_mult_coefficients(MatrixType& delta,
                                   Matrix<typename MatrixType::Scalar, Dynamic, 1>& mult_coefficients) {
    auto p = delta.rows();
    // Divide first two columns
    for (int i = 2; i < p; ++i) {
        delta(i, 0) *= mult_coefficients(i - 2);
    }

    for (int i = 1; i < p; ++i) {
        delta(i, 1) *= mult_coefficients(i - 1);
    }

    // Divide remaining columns.
    for (int j = 2; j < p; ++j) {
        for (int i = 0; i < p; ++i) {
            delta(i, j) *= mult_coefficients(i + j - 2);
        }
    }
}

template <typename Scalar>
struct DeltaMatrixDeterminant {
    using VectorType = Matrix<Scalar, Dynamic, 1>;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;
    Scalar operator()(Scalar alpha) {
        MatrixType copy = matrix;

        auto mult_coefficients = delta_mult_coefficients(alpha, matrix.rows());
        delta_apply_mult_coefficients(copy, mult_coefficients);

        return copy.determinant();
    }

    MatrixType matrix;
};

template <typename VectorType>
typename VectorType::Scalar lambda_tilde(VectorType& moments, int p) {
    using Scalar = typename VectorType::Scalar;

    // This is the closed solution for lambda_1
    Scalar last_lambda = moments(1) / (moments(0) * moments(0)) - 1;
    for (auto i = 2; i <= p; ++i) {
        DeltaMatrixDeterminant<Scalar> mdet{/*.matrix = */ delta_matrix_template(moments, i + 1)};
        last_lambda = util::uniroot(mdet, static_cast<Scalar>(0), last_lambda, static_cast<Scalar>(1e-9), 1000);
    }

    return last_lambda;
}

template <typename VectorType>
Matrix<typename VectorType::Scalar, Dynamic, 1> mu_roots(VectorType& moments,
                                                         typename VectorType::Scalar lambda_tilde,
                                                         int p) {
    using Scalar = typename VectorType::Scalar;
    using VecType = Matrix<Scalar, Dynamic, 1>;

    auto M = delta_matrix_template(moments, p + 1);
    auto mult_coefficients = delta_mult_coefficients(lambda_tilde, p + 1);
    delta_apply_mult_coefficients(M, mult_coefficients);

    VecType poly_coeffs(p + 1);

    M.col(p) = VectorType::Zero(p + 1);

    for (int i = p; i >= 0; --i) {
        M(i, p) = 1;
        poly_coeffs(p - i) = M.determinant();
        M(i, p) = 0;
    }

    VecType real_roots = VecType::Zero(p);
    VecType complex_roots = VecType::Zero(p);

    util::RPoly<Scalar> poly_solver;
    poly_solver.findRoots(poly_coeffs.data(), p, real_roots.data(), complex_roots.data());

    return real_roots;
}

template <typename VectorType>
VectorType mixture_proportions(VectorType& mu, VectorType& moments, typename VectorType::Scalar lambda_tilde, int p) {
    using Scalar = typename VectorType::Scalar;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;

    MatrixType vandermonde(p, p);

    vandermonde.row(0) = VectorType::Ones(p);
    vandermonde.row(1) = mu;
    vandermonde.row(2) = mu.cwiseProduct(mu);
    for (int i = 3; i < p; ++i) {
        vandermonde.row(i) = mu.array().pow(i);
    }

    VectorType delta_vec(p);
    delta_vec(0) = 1;
    delta_vec(1) = moments(0);
    delta_vec(2) = moments(1) / (1 + lambda_tilde);
    delta_vec(3) = moments(2) / ((1 + lambda_tilde) * (1 + 2 * lambda_tilde));

    auto mult_coeff = (1 + lambda_tilde) * (1 + 2 * lambda_tilde);
    for (int i = 4; i < p; ++i) {
        mult_coeff *= (1 + (i - 1) * lambda_tilde);
        delta_vec(i) = moments(i - 1) * (1. / mult_coeff);
    }

    return vandermonde.colPivHouseholderQr().solve(delta_vec);
}

template <typename VectorType>
typename VectorType::Scalar lpb4_cdf(VectorType& prop,
                                     VectorType& mu,
                                     typename VectorType::Scalar lambda_tilde,
                                     typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k = 1. / lambda_tilde;

    Scalar res = 0;
    for (int i = 0; i < prop.rows(); ++i) {
        auto theta = mu(i) * lambda_tilde;

        if (theta <= 0) {
            throw std::runtime_error("Wrong theta parameter.");
        }

        gamma_distribution<Scalar> gamma(k, theta);
        res += prop(i) * cdf(gamma, quantile);
    }

    return res;
}

template <typename VectorType>
typename VectorType::Scalar lpb4_cdf_complement(VectorType& prop,
                                                VectorType& mu,
                                                typename VectorType::Scalar lambda_tilde,
                                                typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k = 1. / lambda_tilde;
    Scalar res = 0;
    for (int i = 0; i < prop.rows(); ++i) {
        auto theta = mu(i) * lambda_tilde;
        gamma_distribution<Scalar> gamma(k, theta);
        res += prop(i) * cdf(complement(gamma, quantile));
    }

    return res;
}

}  // namespace detail

/**
 * A comparison of efficient approximations for a weighted sum of chi-squared random variables
 */
template <typename VectorType>
typename VectorType::Scalar lpb4(VectorType& coeffs, typename VectorType::Scalar quantile) {
    if (coeffs.rows() < 4) {
        throw std::invalid_argument("lbp4 requires at least 4 coefficients.");
    }

    auto p = 4;
    auto moments = detail::chisquaresum_moments(coeffs, p);
    auto ld_tilde = detail::lambda_tilde(moments, p);
    auto mu = detail::mu_roots(moments, ld_tilde, p);
    auto prop = detail::mixture_proportions(mu, moments, ld_tilde, p);
    return detail::lpb4_cdf(prop, mu, ld_tilde, quantile);
}

template <typename VectorType>
typename VectorType::Scalar lpb4_complement(VectorType& coeffs, typename VectorType::Scalar quantile) {
    if (coeffs.rows() < 4) {
        throw std::invalid_argument("lbp4 requires at least 4 coefficients.");
    }

    auto p = 4;
    auto moments = detail::chisquaresum_moments(coeffs, p);
    auto ld_tilde = detail::lambda_tilde(moments, p);
    auto mu = detail::mu_roots(moments, ld_tilde, p);
    auto prop = detail::mixture_proportions(mu, moments, ld_tilde, p);
    return detail::lpb4_cdf_complement(prop, mu, ld_tilde, quantile);
}

template <typename VectorType>
typename VectorType::Scalar hbe(VectorType& coeffs, typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k1 = coeffs.sum();
    auto squared = coeffs.array().square().matrix();
    auto k2 = 2 * squared.sum();
    auto k3 = 8 * (coeffs.dot(squared));

    auto nu = 8 * (k2 * k2 * k2) / (k3 * k3);

    auto statistic = std::sqrt(2 * nu / k2) * (quantile - k1) + nu;

    gamma_distribution<Scalar> gamma(nu / 2., 2);

    return cdf(gamma, statistic);
}

template <typename VectorType>
typename VectorType::Scalar hbe_complement(VectorType& coeffs, typename VectorType::Scalar quantile) {
    using Scalar = typename VectorType::Scalar;
    auto k1 = coeffs.sum();
    auto squared = coeffs.array().square().matrix();
    auto k2 = 2 * squared.sum();
    auto k3 = 8 * (coeffs.dot(squared));

    auto nu = 8 * (k2 * k2 * k2) / (k3 * k3);

    auto statistic = std::sqrt(2 * nu / k2) * (quantile - k1) + nu;

    gamma_distribution<Scalar> gamma(nu / 2., 2);

    return cdf(complement(gamma, statistic));
}

}  // namespace util

#endif  // PYBNESIAN_UTIL_CHISQUARESUM_HPP