/*
 *  utils.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2016-11-08
 *
 *  Useful functions to provide compatibility
 *  of basic datatypes, mainly in matematical
 *  calculations.
 *
 */

#ifndef DVRK_UTILS_HPP_
#define DVRK_UTILS_HPP_

#include <iostream>
#include <sstream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <limits>
#include "irob_utils/pose.hpp"

#include <std_msgs/Float32.h>
#include <irob_msgs/FloatArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <irob_msgs/ToolPose.h>
#include <irob_msgs/ToolPoseStamped.h>
#include <irob_msgs/Environment.h>

namespace saf {

typedef enum InterpolationMethod
{LINEAR, BEZIER} InterpolationMethod;

inline double degToRad(double deg) {
  return (deg / 180.0) * M_PI;
}

inline double radToDeg(double rad) {
  return (rad * 180.0) / M_PI;
}

template<typename T>
std::ostream& operator<<(std::ostream& out, const std::vector<T>& v) {
  out << "[";
  size_t last = v.size() - 1;
  for(size_t i = 0; i < v.size(); ++i) {
    out << v[i];
    if (i != last)
      out << ", ";
  }
  out << "]";
  return out;
}

// Interpolation
template<typename T>
inline T interpolate(double a, T const& x1, T const& x2) {
  return ((1.0-a) * x1) + ((a) * x2);
}

template <>
inline Pose interpolate(double a, const Pose& x1, const Pose& x2) {
  return x1.interpolate(a, x2);
}

template <>
inline Eigen::Quaternion<double> interpolate(double a,
                                             const Eigen::Quaternion<double>& x1,
                                             const Eigen::Quaternion<double>& x2) {
  return x1.slerp(a, x2);
}

// Distance
template<typename T>
inline double distanceEuler(T const& x1, T const& x2) {
  return std::abs(x2 - x1);
}

template <>
inline double distanceEuler(const Pose& x1, const Pose& x2) {
  Pose::Distance d = x1.dist(x2);
  double weighted_cartesian = std::abs(d.cartesian) * 10000.0;
  double weighted_angle = std::abs(d.angle);
  double weighted_jaw = radToDeg(std::abs(d.jaw));
  if (weighted_cartesian >= weighted_angle
      && weighted_cartesian >= weighted_jaw)
    return std::abs(d.cartesian);
  if (weighted_angle >= weighted_jaw)
    // in degrees, should be converted to rad?
    return std::abs(d.angle);
  return std::abs(d.jaw);
}

template <>
inline double distanceEuler(const Eigen::Vector3d& x1,
                            const Eigen::Vector3d& x2) {
  return std::abs((x2-x1).norm());
}

// Conversion from ROS msg
template<typename MsgT, typename DataT>
inline DataT unwrapMsg(const MsgT& msg);

template <>
inline irob_msgs::Environment unwrapMsg(const irob_msgs::Environment& msg){
  return msg;
}

template <>
inline double unwrapMsg(const std_msgs::Float32& msg){
  return msg.data;
}

template <>
inline Pose unwrapMsg(const geometry_msgs::Pose& msg){
  Pose ret(msg, 0);
  return ret;
}

template <>
inline Pose unwrapMsg(const irob_msgs::ToolPose& msg){
  Pose ret(msg);
  return ret;
}

template <>
inline Pose unwrapMsg(const geometry_msgs::PoseStamped& msg){
  Pose ret(msg, 0);
  return ret;
}

template <>
inline Pose unwrapMsg(const irob_msgs::ToolPoseStamped& msg){
  Pose ret(msg);
  return ret;
}

template <>
inline Eigen::Vector3d unwrapMsg(const geometry_msgs::Point& msg){
  Eigen::Vector3d ret(msg.x, msg.y, msg.z);
  return ret;
}

template <>
inline Eigen::Vector3d unwrapMsg(const geometry_msgs::PointStamped& msg){
  Eigen::Vector3d ret(msg.point.x, msg.point.y, msg.point.z);
  return ret;
}

template <>
inline Eigen::Quaternion<double> unwrapMsg(const geometry_msgs::Quaternion& msg){
  Eigen::Quaternion<double> ret(msg.w, msg.x, msg.y, msg.z);
  return ret;
}

template <>
inline std::vector<double> unwrapMsg(const irob_msgs::FloatArray& msg){
  return msg.data;
}

// Conversion to ROS msg
template<typename MsgT, typename DataT>
inline MsgT wrapToMsg(const DataT& data);

template <>
inline std_msgs::Float32 wrapToMsg(const double& data){
  std_msgs::Float32 msg;
  msg.data = data;
  return msg;
}

template <>
inline geometry_msgs::Pose wrapToMsg(const Pose& data){
  return data.toRosPose();
}

template <>
inline irob_msgs::ToolPose wrapToMsg(const Pose& data){
  return data.toRosToolPose();
}

template <>
inline geometry_msgs::Point wrapToMsg(const Eigen::Vector3d& data){
  geometry_msgs::Point msg;
  msg.x = data.x();
  msg.y = data.y();
  msg.z = data.z();
  return msg;
}

template <>
inline geometry_msgs::PointStamped wrapToMsg(const Eigen::Vector3d& data){
  geometry_msgs::PointStamped msg;
  msg.point.x = data.x();
  msg.point.y = data.y();
  msg.point.z = data.z();
  return msg;
}

template <>
inline geometry_msgs::Quaternion wrapToMsg(
    const Eigen::Quaternion<double>& data){
  geometry_msgs::Quaternion msg;
  msg.w = data.w();
  msg.x = data.x();
  msg.y = data.y();
  msg.z = data.z();
  return msg;
}

// NaN
template<typename DataT>
inline DataT makeNaN();

template <>
inline double makeNaN(){
  return std::numeric_limits<double>::quiet_NaN();
}

template <>
inline Pose makeNaN(){
  Pose ret(std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN());
  return ret;
}

template <>
inline Eigen::Vector3d makeNaN(){
  Eigen::Vector3d ret(std::numeric_limits<double>::quiet_NaN(),
                      std::numeric_limits<double>::quiet_NaN(),
                      std::numeric_limits<double>::quiet_NaN());
  return ret;
}

template <>
inline Eigen::Quaternion<double> makeNaN(){
  Eigen::Quaternion<double> ret(std::numeric_limits<double>::quiet_NaN(),
                                std::numeric_limits<double>::quiet_NaN(),
                                std::numeric_limits<double>::quiet_NaN(),
                                std::numeric_limits<double>::quiet_NaN());
  return ret;
}

template <>
inline std_msgs::Float32 makeNaN(){
  std_msgs::Float32 msg;
  msg.data = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

template <>
inline irob_msgs::FloatArray makeNaN(){
  irob_msgs::FloatArray msg;
  msg.data.push_back(std::numeric_limits<double>::quiet_NaN());
  return msg;
}

template <>
inline geometry_msgs::Pose makeNaN(){
  Pose nanp = makeNaN<Pose>();
  return nanp.toRosPose();
}

template <>
inline irob_msgs::ToolPose makeNaN(){
  Pose nanp = makeNaN<Pose>();
  return nanp.toRosToolPose();
}

template <>
inline irob_msgs::Environment makeNaN(){
  irob_msgs::Environment nanp;
  nanp.valid = irob_msgs::Environment::INVALID;
  return nanp;
}

template <>
inline geometry_msgs::Point makeNaN(){
  geometry_msgs::Point msg;
  msg.x = std::numeric_limits<double>::quiet_NaN();
  msg.y = std::numeric_limits<double>::quiet_NaN();
  msg.z = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

template <>
inline geometry_msgs::PointStamped makeNaN(){
  geometry_msgs::PointStamped msg;
  msg.point.x = std::numeric_limits<double>::quiet_NaN();
  msg.point.y = std::numeric_limits<double>::quiet_NaN();
  msg.point.z = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

template <>
inline geometry_msgs::Quaternion makeNaN(){
  geometry_msgs::Quaternion msg;
  msg.w = std::numeric_limits<double>::quiet_NaN();
  msg.x = std::numeric_limits<double>::quiet_NaN();
  msg.y = std::numeric_limits<double>::quiet_NaN();
  msg.z = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

// isnan
template<typename DataT>
inline bool isnan(const DataT& d);

template <>
inline bool isnan(const double& d)
{
  return std::isnan(d);
}

template <>
inline bool isnan(const Pose& d)
{
  return (std::isnan(d.position.x())
          || std::isnan(d.position.y())
          || std::isnan(d.position.z())
          || std::isnan(d.orientation.x())
          || std::isnan(d.orientation.y())
          || std::isnan(d.orientation.z())
          || std::isnan(d.orientation.w())
          || std::isnan(d.jaw));
}

template <>
inline bool isnan(const Eigen::Vector3d& d)
{
  return (std::isnan(d.x())
          || std::isnan(d.y())
          || std::isnan(d.z()));
}

template <>
inline bool isnan(const Eigen::Quaternion<double>& d)
{
  return (std::isnan(d.x())
          || std::isnan(d.y())
          || std::isnan(d.z())
          || std::isnan(d.w()));
}

template <>
inline bool isnan(const std_msgs::Float32& d)
{

  return (std::isnan(d.data));
}

template <>
inline bool isnan(const geometry_msgs::Pose& d)
{
  return (std::isnan(d.position.x)
          || std::isnan(d.position.y)
          || std::isnan(d.position.z)
          || std::isnan(d.orientation.x)
          || std::isnan(d.orientation.y)
          || std::isnan(d.orientation.z)
          || std::isnan(d.orientation.w));
}

template <>
inline bool isnan(const irob_msgs::ToolPose& d)
{
  return (std::isnan(d.position.x)
          || std::isnan(d.position.y)
          || std::isnan(d.position.z)
          || std::isnan(d.orientation.x)
          || std::isnan(d.orientation.y)
          || std::isnan(d.orientation.z)
          || std::isnan(d.orientation.w)
          || std::isnan(d.jaw));
}

template <>
inline bool isnan(const geometry_msgs::Point& d)
{
  return (std::isnan(d.x)
          || std::isnan(d.y)
          || std::isnan(d.z) );
}

template <>
inline bool isnan(const geometry_msgs::PointStamped& d)
{
  return isnan<geometry_msgs::Point>(d.point);
}

template <>
inline bool isnan(const geometry_msgs::Quaternion& d)
{
  return (std::isnan(d.x)
          || std::isnan(d.y)
          || std::isnan(d.z)
          || std::isnan(d.w));
}

// Unit vector + rotation to quat
template<typename QuatT, typename VecT>
inline QuatT vecToQuat(const VecT& vec, double angle);

template <>
inline Eigen::Quaternion<double> vecToQuat(const Eigen::Vector3d& vec,
                                           double angle){
  Eigen::Quaternion<double> quat_start(0.0, 0.707107, 0.707106, 0.0);
  double angle_rad = (angle / 180.0) * M_PI;
  Eigen::Matrix3d R1m;
  R1m = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(0.0,  Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitZ());
  Eigen::Quaternion<double> R1(R1m);

  Eigen::Quaternion<double> ret = R1 * quat_start;

  Eigen::Vector3d vec_start(0.0, 0.0, -1.0);
  Eigen::Quaternion<double> R2 =
      Eigen::Quaternion<double>::FromTwoVectors(vec_start, vec);
  ret = R2 * ret;
  return ret;
}

// Quat to unit vector
template<typename QuatT, typename VecT>
inline VecT quatToVec(const QuatT& quat);

template <>
inline Eigen::Vector3d quatToVec(const Eigen::Quaternion<double>& quat){

  Eigen::Quaternion<double> quat_start(0.0, 0.707107, 0.707106, 0.0);

  Eigen::Quaternion<double> R = quat * quat_start.inverse();

  Eigen::Vector3d vec_start(0.0, 0.0, -1.0);
  Eigen::Vector3d ret = R * vec_start;
  return ret;
}

}

#endif /* DVRK_UTILS_HPP_ */


/*
* Implementation for AD of convolution-related operations, that will typically
* be used in a CNN. This also includes corresponding node types.
*/

#pragma once

#include "autodiff.hpp"
#include "utils.hpp"

#include <vector>
#include <memory>
#include <iostream>

#include <Eigen/Dense>

// Enum for channel splitting directions in CNN
// (declared outside for now because scoped enum declarationb seems
// impossible)
enum class ts::ChannelSplit : int {
	NOSPLIT,
	SPLIT_HOR,	// Splits lines
	SPLIT_VERT	// Splits columns
};

namespace ts {

	template <typename T>
	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> convArray(
		const Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &mat,
		const Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &ker
	);

	template <typename T> class ConvolutionNode;
	template <typename T>
	ts::Tensor<T> convolution(const ts::Tensor<T> &mat, const ts::Tensor<T> &ker);

	template <typename T> class SplitNode;
	template <typename T>
	std::vector<ts::Tensor<T>> split(
		const ts::Tensor<T> &x,
		ChannelSplit channelSplit,
		unsigned nInputChannels
	);

	template <typename T> class PoolingNode;
	template <typename T>
	ts::Tensor<T> maxPooling(const ts::Tensor<T> &x, std::vector<unsigned> pool);

	template <typename T> class VertCatNode;
	template <typename T>
	ts::Tensor<T> vertCat(const std::vector<ts::Tensor<T>> &x);

	template <typename T> class FlatteningNode;
	template <typename T>
	ts::Tensor<T> flattening(const ts::Tensor<T> &x);

	template <typename T> class Im2ColNode;
	template <typename T>
	ts::Tensor<T> im2col(
		const std::vector<ts::Tensor<T>> &x,
		std::vector<unsigned> kernelDim
	);

	template <typename T> class Col2ImNode;
	template <typename T>
	std::vector<ts::Tensor<T>> col2im(
		const ts::Tensor<T> &x,
		std::vector<unsigned> outputDim
	);
}

	// ts::ConvolutionNode

template <typename T>
class ts::ConvolutionNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);
};

	// ts::PoolingNode

template <typename T>
class ts::PoolingNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	PoolingNode(
		std::vector<long> shape,
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> xVal, int xDep,
		std::vector<unsigned> newPool
	);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	std::vector<unsigned> pool = {};

	friend ts::Tensor<T> ts::maxPooling<>(
		const ts::Tensor<T> &x, std::vector<unsigned> pool
	);
};

	// ts::SplitNode

template <typename T>
class ts::SplitNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	SplitNode(
		std::vector<long> shape,
		int xDep,
		std::vector<long> originalShape,
		ChannelSplit newSplitDirection,
		unsigned newPosition
	);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	long originalRows, originalCols;
	ChannelSplit splitDirection;
	unsigned position;

	friend std::vector<ts::Tensor<T>> ts::split<>(
		const ts::Tensor<T> &x,
		ChannelSplit channelSplit,
		unsigned nInputChannels
	);
};

	// ts::VertCatNode

template <typename T>
class ts::VertCatNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	// This node can have n parents !
	VertCatNode(
		std::vector<long> shape,
		std::vector<int> newDependencies,
		std::vector<long> newHeights
	);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	std::vector<long> heights = {};

	friend ts::Tensor<T> ts::vertCat<>(const std::vector<ts::Tensor<T>> &x);
};

	// ts::FlatteningNode

template <typename T>
class ts::FlatteningNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	FlatteningNode(
		std::vector<long> shape,
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> xVal, int xDep,
		std::vector<long> newSize
	);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	std::vector<long> size = {};

	friend ts::Tensor<T> ts::flattening<>(const ts::Tensor<T> &x);
};

	// ts::Im2ColNode

template <typename T>
class ts::Im2ColNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	// This node can have n parents !
	Im2ColNode(
		std::vector<long> shape,
		std::vector<int> newDependencies,
		std::vector<long> newKernelDim,
		std::vector<long> newMatrixDim,
		unsigned newNChannels
	);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	std::vector<long> kernelDim = {};
	std::vector<long> matrixDim = {};	// Size of one channel
	unsigned nChannels;	// Input nChannels

	friend ts::Tensor<T> ts::im2col<>(
		const std::vector<ts::Tensor<T>> &x,
		std::vector<unsigned> kernelDim
	);
};

	// ts::Col2ImNode

template <typename T>
class ts::Col2ImNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	// This node can have n parents !
	Col2ImNode(
		std::vector<long> shape,
		int xDep,
		unsigned newPosition,
		long newNChannels
	);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	unsigned position;
	unsigned nChannels;

	friend std::vector<ts::Tensor<T>> ts::col2im<>(
		const ts::Tensor<T> &x,
		std::vector<unsigned> outputDim
	) ;
};


// [[Rcpp::depends(BH)]]

#include <cmath>
#include <boost/math/special_functions/beta.hpp>

#include "include/lm.h"

double pval_t(double t, double df) {
    if (df > 50) {
        return 1 + boost::math::erf(-std::abs(t) / sqrt(2));
    }
    double x = df / (t * t + df);
    return boost::math::ibeta(df / 2, 0.5, x);
}

lm::lm(unsigned _n) :lt(_n * 2), b(_n), v(_n), vM(_n), R(4), Q(_n * 2), n(_n) {}

double lm::norm(const std::vector<double>& v) {
    double value = 0.0;
    for (double x: v) {
        value += x * x;
    }
    return std::sqrt(value);
}

void lm::set(int i, double x, double y, double w, bool bias) {
    double sq = std::sqrt(w);
    if (bias) {
        lt[i] = sq;
    }
    lt[i + n] = x * sq;
    b[i] = y * sq;
}

void lm::solve() {
    preprocess(v, lt, 0, n, 0);

    // Q_1 * A
    std::vector<double> RM(n);
    double cross = 0.0;
    for (int j = 0; j < n; j++) {
        cross += v[j] * lt[n + j];
    }
    for (int i = 1; i < n; i++) {
        RM[i] = lt[n + i] - 2 * v[i] * cross;
    }

    vM[0] = 0;
    preprocess(vM, RM, 0, n, 1);
    compute_Q();
    compute_R();
    solve_system();
}

void lm::preprocess(std::vector<double>& ret, const std::vector<double>& lt, int l, int r, int s) {
    int n = r - l;
    for (int i = s; i < n; i++) {
        ret[i] = lt[i + l];
    }
    ret[s] -= norm(ret);
    double l2 = norm(ret);
    if (l2 > 0) {
        for (int i = 0; i < n; i++) {
            ret[i] /= l2;
        }
    }
}

void lm::compute_Q() {
    double cross = 0.0;
    for (int k = 0; k < n; k++) {
        cross += v[k] * vM[k];
    }
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < 2; j++) {
            double sum = 0;
            sum += 4 * v[i] * vM[j] * cross;
            Q[i + j * n] = sum - 2 * (v[i] * v[j] + vM[i] * vM[j]);
        }
    }
    Q[0] += 1;
    Q[n + 1] += 1;
}

void lm::compute_R() {
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            if (i == 1 && j == 0) {
                continue;
            }
            double sum = 0;
            for (int k = 0; k < n; k++) {
                sum += Q[k + i * n] * lt[k + j * n];
            }
            R[i + j * 2] = sum;
        }
    }
    R[1] = 0;

}

void lm::solve_system() {
    double b1 = 0.0;
    double b2 = 0.0;
    for (int i = 0; i < n; i++) {
        b1 += Q[i] * b[i];
        b2 += Q[i + n] * b[i];
    }
    k = b2 / R[3];
    bias = (b1 - k * R[2]) / R[0];
}

double lm::compute_t(double df) {
    double a = R[0] * R[0];
    double b = R[0] * R[2];
    double c = R[2] * R[2] + R[3] * R[3];
    double inv =  a / (a * c - b * b);
    return k / std::sqrt((compute_rss() / df) * inv);

}

double lm::compute_rss() {
    double rss = 0;
    for (int i = 0; i < n; i++) {
        rss += pow(bias * lt[i] + k * lt[n + i] - b[i], 2.0);
    }
    return rss;
}

double lm::get_lambda() {
    return k;
}

double lm::get_bias() {
    return bias;
}


// Petter Strandmark 2012.

#include <algorithm>
#include <cstdio>
#include <iostream>
#include <limits>
#include <stdexcept>
#include <vector>

#include <Eigen/Dense>

#include <spii/spii.h>
#include <spii/solver.h>

namespace spii {

// Holds a point in the Nelder-Mead simplex.
// Equipped with a comparison operator for sorting.
struct SimplexPoint
{
	Eigen::VectorXd x;
	double value;

	bool operator<(const SimplexPoint& rhs) const
	{
		return this->value < rhs.value;
	}
};

// If required for debugging.
std::ostream& operator<<(std::ostream& out, const SimplexPoint& point)
{
	out << point.x.transpose() << " : " << point.value;
	return out;
}

}  // namespace spii

namespace std
{
	template<>
	void swap<spii::SimplexPoint>(spii::SimplexPoint& lhs, spii::SimplexPoint& rhs)
	{
		lhs.x.swap(rhs.x);
		swap(lhs.value, rhs.value);
	}
}

namespace spii {

void initialize_simplex(const Function& function,
                        const Eigen::VectorXd& x0,
                        std::vector<SimplexPoint>* simplex)
{
	size_t n = function.get_number_of_scalars();
	Eigen::VectorXd absx0 = x0;
	for (size_t i = 0; i < n; ++i) {
		absx0[i] = std::abs(x0[i]);
	}
	double scale = std::max(absx0.maxCoeff(), 1.0);
	const double nd = static_cast<double>(n);
	double alpha1 = scale / (nd * std::sqrt(2.0)) * (std::sqrt(nd+1)- 1 + nd);
	double alpha2 = scale / (nd * std::sqrt(2.0)) * (std::sqrt(nd+1) - 1);
	Eigen::VectorXd alpha2_vec(x0.size());
	alpha2_vec.setConstant(alpha2);

	simplex->at(0).x = x0;
	for (size_t i = 1; i < n + 1; ++i) {
		simplex->at(i).x  = x0 + alpha2_vec;
		simplex->at(i).x[i-1] = x0[i-1] + alpha1;
	}

	for (size_t i = 0; i < n + 1; ++i) {
		simplex->at(i).value = function.evaluate(simplex->at(i).x);
	}

	std::sort(simplex->begin(), simplex->end());
}

void NelderMeadSolver::solve(const Function& function,
                             SolverResults* results) const
{
	double global_start_time = wall_time();

	// Dimension of problem.
	size_t n = function.get_number_of_scalars();

	if (n == 0) {
		results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
		return;
	}

	// The Nelder-Mead simplex.
	std::vector<SimplexPoint> simplex(n + 1);

	// Copy the user state to the current point.
	Eigen::VectorXd x;
	function.copy_user_to_global(&x);

	initialize_simplex(function, x, &simplex);

	SimplexPoint mean_point;
	SimplexPoint reflection_point;
	SimplexPoint expansion_point;
	mean_point.x.resize(n);
	reflection_point.x.resize(n);
	expansion_point.x.resize(n);

	double fmin  = std::numeric_limits<double>::quiet_NaN();
	double fmax  = std::numeric_limits<double>::quiet_NaN();
	double fval  = std::numeric_limits<double>::quiet_NaN();
	double area  = std::numeric_limits<double>::quiet_NaN();
	double area0 = std::numeric_limits<double>::quiet_NaN();
	double length  = std::numeric_limits<double>::quiet_NaN();
	double length0 = std::numeric_limits<double>::quiet_NaN();

	Eigen::MatrixXd area_mat(n, n);

	//
	// START MAIN ITERATION
	//
	results->startup_time   += wall_time() - global_start_time;
	results->exit_condition = SolverResults::INTERNAL_ERROR;
	int iter = 0;
	int n_shrink_in_a_row = 0;
	while (true) {

		//
		// In each iteration, the worst point in the simplex
		// is replaced with a new one.
		//
		double start_time = wall_time();

		mean_point.x.setZero();
		fval = 0;
		// Compute the mean of the best n points.
		for (size_t i = 0; i < n; ++i) {
			mean_point.x += simplex[i].x;
			fval         += simplex[i].value;
		}
		fval         /= double(n);
		mean_point.x /= double(n);
		fmin = simplex[0].value;
		fmax = simplex[n].value;

		const char* iteration_type = "n/a";

		// Compute the reflexion point and evaluate it.
		reflection_point.x = 2.0 * mean_point.x - simplex[n].x;
		reflection_point.value = function.evaluate(reflection_point.x);

		bool is_shrink = false;
		if (simplex[0].value <= reflection_point.value &&
			reflection_point.value < simplex[n - 1].value) {
			// Reflected point is neither better nor worst in the
			// new simplex.
			std::swap(reflection_point, simplex[n]);
			iteration_type = "Reflect 1";
		}
		else if (reflection_point.value < simplex[0].value) {
			// Reflected point is better than the current best; try
			// to go farther along this direction.

			// Compute expansion point.
			expansion_point.x = 3.0 * mean_point.x - 2.0 * simplex[n].x;
			expansion_point.value = function.evaluate(expansion_point.x);

			if (expansion_point.value < reflection_point.value) {
				std::swap(expansion_point, simplex[n]);
				iteration_type = "Expansion";
			}
			else {
				std::swap(reflection_point, simplex[n]);
				iteration_type = "Reflect 2";
			}
		}
		else {
			// Reflected point is still worse than x[n]; contract.
			bool success = false;

			if (simplex[n - 1].value <= reflection_point.value &&
			    reflection_point.value < simplex[n].value) {
				// Try to perform "outside" contraction.
				expansion_point.x = 1.5 * mean_point.x - 0.5 * simplex[n].x;
				expansion_point.value = function.evaluate(expansion_point.x);

				if (expansion_point.value <= reflection_point.value) {
					std::swap(expansion_point, simplex[n]);
					success = true;
					iteration_type = "Outside contraction";
				}
			}
			else {
				// Try to perform "inside" contraction.
				expansion_point.x = 0.5 * mean_point.x + 0.5 * simplex[n].x;
				expansion_point.value = function.evaluate(expansion_point.x);

				if (expansion_point.value < simplex[n].value) {
					std::swap(expansion_point, simplex[n]);
					success = true;
					iteration_type = "Inside contraction";
				}
			}

			if (! success) {
				// Neither outside nor inside contraction was acceptable;
				// shrink the simplex toward the best point.
				for (size_t i = 1; i < n + 1; ++i) {
					simplex[i].x = 0.5 * (simplex[0].x + simplex[i].x);
					simplex[i].value = function.evaluate(simplex[i].x);
					iteration_type = "Shrink";
					is_shrink = true;
				}
			}
		}

		std::sort(simplex.begin(), simplex.end());

		results->function_evaluation_time += wall_time() - start_time;

		//
		// Test stopping criteriea
		//
		start_time = wall_time();

		// Compute the area of the simplex.
		length = 0;
		for (size_t i = 0; i < n; ++i) {
			area_mat.col(i) = simplex[i].x - simplex[n].x;
			length = std::max(length, area_mat.col(i).norm());
		}
		area = std::abs(area_mat.determinant());
		if (iter == 0) {
			area0 = area;
			length0 = length;
		}

		if (area / area0 < this->area_tolerance) {
			results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
			break;
		}

		if (area == 0) {
			results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
			break;
		}

		if (length / length0 < this->length_tolerance) {
			results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
			break;
		}

		if (is_shrink) {
			n_shrink_in_a_row++;
		}
		else {
			n_shrink_in_a_row = 0;
		}
		if (n_shrink_in_a_row > 50) {
			results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
			break;
		}

		if (iter >= this->maximum_iterations) {
			results->exit_condition = SolverResults::NO_CONVERGENCE;
			break;
		}

		if (this->callback_function) {
			CallbackInformation information;
			information.objective_value = simplex[0].value;
			information.x = &simplex[0].x;

			if (!callback_function(information)) {
				results->exit_condition = SolverResults::USER_ABORT;
				break;
			}
		}
		results->stopping_criteria_time += wall_time() - start_time;

		//
		// Restarting
		//
		//if (area / area1 < 1e-10) {
		//	x = simplex[0].x;
		//	initialize_simplex(function, x, &simplex);
		//	area1 = area;
		//	if (this->log_function) {
		//		this->log_function("Restarted.");
		//	}
		//}

		//
		// Log the results of this iteration.
		//
		start_time = wall_time();

		int log_interval = 1;
		if (iter > 30) {
			log_interval = 10;
		}
		if (iter > 200) {
			log_interval = 100;
		}
		if (iter > 2000) {
			log_interval = 1000;
		}
		if (this->log_function && iter % log_interval == 0) {
			char str[1024];
				if (iter == 0) {
					this->log_function("Itr     min(f)     avg(f)     max(f)    area    length   type");
				}
				std::sprintf(str, "%6d %+.3e %+.3e %+.3e %.3e %.3e %s",
					iter, fmin, fval, fmax, area, length, iteration_type);
			this->log_function(str);
		}
		results->log_time += wall_time() - start_time;

		iter++;
	}

	// Return the best point as solution.
	function.copy_global_to_user(simplex[0].x);
	results->total_time += wall_time() - global_start_time;

	if (this->log_function) {
		char str[1024];
		std::sprintf(str, " end   %+.3e                       %.3e %.3e", fval, area, length);
		this->log_function(str);
	}
}

}  // namespace spii


/**
 * @file particle.hpp
 * @author Vahid Bastani
 *
 * Generic particle filter.
 */
#ifndef SSMPACK_FILTER_PARTICLE_HPP
#define SSMPACK_FILTER_PARTICLE_HPP

#include "ssmkit/distribution/conditional.hpp"
#include "ssmkit/filter/recursive_bayesian_base.hpp"
#include "ssmkit/process/hierarchical.hpp"
#include "ssmkit/process/markov.hpp"
#include "ssmkit/process/memoryless.hpp"
#include <armadillo>

#include <tuple>

namespace ssmkit {
namespace filter {

using process::Hierarchical;
using process::Markov;
using process::Memoryless;
using distribution::Conditional;

/** Particle Filter.
 */
template <class Process, class Resampler>
class Particle : public RecursiveBayesianBase<Particle<Process, Resampler>> {
 public:
  /** Type of the state posterior
   *
   * \f$ \{\mathbf{x}^{(i)}_t,\omega^{(i)}\}_{i=1}^{M}\f$
   */
  using CompeleteState = std::tuple<arma::mat, arma::vec>;

 private:
  //! Particle weights \f$ \{\omega^{(i)}\}_{i=1}^{M}\f$.
  arma::vec w_;
  //! State particles \f$ \{\mathbf{x}^{(i)}_t\}_{i=1}^{M}\f$.
  arma::mat state_par_;
  //! The process model
  Process process_;
  //! Resampling algorithm
  Resampler resampler_;
  //! Number of particles \f$M\f$.
  unsigned long num_;

 private:
  //! Normalizes the sum of weights to one
  void normalizeWeights(void) { w_ = w_ / arma::sum(w_); }

 public:
  /** Constructor
   *
   * returns a Particle filter object.
   *
   * @param process The process model object that the PF is defined for
   * @param resampler The resampling algorithm
   * @param particles_num Number of particles \f$M\f$
   */
  Particle(Process process, Resampler resampler, unsigned long particles_num)
      : process_{process}, resampler_{resampler}, num_{particles_num} {
    // initialized w_ and state_par_
    w_.resize(num_);
    // take one sample to find out dimension
    auto tmp = process_.template getProcess<0>().getInitialPDF().random();
    state_par_.resize(tmp.size(), num_);
  }
  /** Prediction
   *
   * Performs prediction step.
   * \f{equation}{\mathbf{x}^{(i)}_t \sim p(\mathbf{x}_t| \tilde{\mathbf{x}}^{(i)}_{t-1}, y^d_1, \cdots, y^d_{N_d})
   * \quad \text{for} \quad i=1,\cdots,M\f}
   *
   * @param args... Control variables \f$y^d_1, \cdots, y^d_{N_d}\f$ of the dynamic process, if any.
   */
  template <class... Args>
  void predict(const Args &... args) {
    state_par_.each_col([this, &args...](arma::vec &v, const Args &... args) {
      v = process_.template getProcess<0>().getCPDF().random(v, args...);
    });
  }
  /** Correction
   *
   * Performs correction step.
   *
   * \f{equation}{ \omega^{(i)} = \tilde{\omega}^{(i)} p(\mathbf{z}_t| \mathbf{x}^{(i)}_t, y^m_1, \cdots, y^m_{N_m}) \f}
   * \f{equation}{\{\mathbf{x}^{(i)}_t,\omega^{(i)}\}_{i=1}^{M}
   * \overset{\mbox{resample}}{\longrightarrow}
   * \{\tilde{\mathbf{x}}^{(i)}_t,\tilde{\omega}^{(i)}\}_{i=1}^{M}\f}
   *
   * @param measurement Measurement vector \f$\mathbf{z}_t\f$.
   * @param args... Control variables \f$y^m_1, \cdots, y^m_{N_m}\f$ of the measurement process, if any.
   * @return Estimated state \f$\{\tilde{\mathbf{x}}^{(i)}_t,\tilde{\omega}^{(i)}\}_{i=1}^{M}\f$
   */
  template <class Measurement, class... TArgs>
  CompeleteState correct(const Measurement &measurement,
                         const TArgs &... args) {
    unsigned long cnt = 0;
    w_.for_each([this, &cnt, &measurement, &args...](double &e) {
      e *= process_.template getProcess<1>().getCPDF().likelihood(
          measurement, state_par_.col(cnt++), args...);
    });

    normalizeWeights();

    resampler_(state_par_, w_);

    return std::make_tuple(state_par_, w_);
  }
  /** Initialization
   *
   * @return Estimated state \f$\{\tilde{\mathbf{x}}^{(i)}_0,\tilde{\omega}^{(i)}\}_{i=1}^{M}\f$
   */
  CompeleteState initialize() {
    state_par_.each_col([this](arma::vec &v) {
      v = process_.template getProcess<0>().getInitialPDF().random();
    });

    unsigned long cnt = 0;
    w_.for_each([this, &cnt](double &e) {
      e = process_.template getProcess<0>().getInitialPDF().likelihood(
          state_par_.col(cnt++));
    });

    normalizeWeights();

    return std::make_tuple(state_par_, w_);
  }
  //! @return Estimated state \f$\{\tilde{\omega}^{(i)}\}_{i=1}^{M}\f$
  const arma::vec &getWeights(void) const { return w_; }
  //! @return Estimated state \f$\{\tilde{\mathbf{x}}^{(i)}_t\}_{i=1}^{M}\f$
  const arma::mat &getStateParticles(void) const { return state_par_; }
};

/**
 */
template <class StatePDF, class StateParamMap, class InitialPDF,
          class MeasurementPDF, class MeasurementParamMap, class Resampler>
auto makeParticle(
    Hierarchical<Markov<StatePDF, StateParamMap, InitialPDF>,
                 Memoryless<MeasurementPDF, MeasurementParamMap>> process,
    Resampler resampler, unsigned long particle_num) {
  return Particle<Hierarchical<Markov<StatePDF, StateParamMap, InitialPDF>,
                               Memoryless<MeasurementPDF, MeasurementParamMap>>,
                  Resampler>(process, resampler, particle_num);
}

} // namespace filter
} // namespace ssmkit

#endif // SSMPACK_FILTER_PARTICLE_HPP


#include <nistpp/math_helpers.h>

#include <boost/math/special_functions/gamma.hpp>

namespace nistpp
{

double igamc(double a, double z)
{
    return  boost::math::gamma_q(a, z);
}

} // namespace nistpp


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 11, 2017 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes
#include <string>

// External includes
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/operators.hpp>

// Project includes
#include "includes/define.h"
#include "custom_utilities/control_point.h"
#include "custom_utilities/control_grid.h"
#include "custom_utilities/fespace.h"
#include "custom_utilities/patch_interface.h"
#include "custom_utilities/isogeometric_utility.h"
#include "custom_utilities/nurbs/domain_manager.h"
#include "custom_utilities/nurbs/domain_manager_2d.h"
#include "custom_utilities/nurbs/structured_control_grid.h"
#include "custom_utilities/nurbs/bsplines_fespace.h"
#include "custom_utilities/nurbs/bsplines_fespace_library.h"
#include "custom_utilities/nurbs/bending_strip_nurbs_patch.h"
#include "custom_utilities/nurbs/nurbs_test_utils.h"
#include "custom_python/iga_define_python.h"
#include "custom_python/add_nurbs_to_python.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

////////////////////////////////////////

template<int TDim, int TWhichDim>
boost::python::list BSplinesFESpace_GetKnotVector(BSplinesFESpace<TDim>& rDummy)
{
    boost::python::list knot_list;

    if (TWhichDim < TDim)
    {
        const typename BSplinesFESpace<TDim>::knot_container_t& knot_vector = rDummy.KnotVector(TWhichDim);

        for (std::size_t i = 0; i < knot_vector.size(); ++i)
            knot_list.append(knot_vector[i]);
    }

    return knot_list;
}

template<int TDim, int TWhichDim>
void BSplinesFESpace_SetKnotVector(BSplinesFESpace<TDim>& rDummy, boost::python::list knot_list)
{
    if (TWhichDim < TDim)
    {
        std::vector<double> knot_vec;
        typedef boost::python::stl_input_iterator<double> iterator_value_type;
        BOOST_FOREACH(const iterator_value_type::value_type& v, std::make_pair(iterator_value_type(knot_list), iterator_value_type() ) )
        {
            knot_vec.push_back(v);
        }

        rDummy.SetKnotVector(TWhichDim, knot_vec);
    }
}

////////////////////////////////////////

BSplinesFESpace<1>::Pointer BSplinesFESpaceLibrary_CreatePrimitiveFESpace1(BSplinesFESpaceLibrary& rDummy, const std::size_t& order_u)
{
    std::vector<std::size_t> orders(1);
    orders[0] = order_u;
    return rDummy.CreatePrimitiveFESpace<1>(orders);
}

BSplinesFESpace<2>::Pointer BSplinesFESpaceLibrary_CreatePrimitiveFESpace2(BSplinesFESpaceLibrary& rDummy, const std::size_t& order_u, const std::size_t& order_v)
{
    std::vector<std::size_t> orders(2);
    orders[0] = order_u;
    orders[1] = order_v;
    return rDummy.CreatePrimitiveFESpace<2>(orders);
}

BSplinesFESpace<3>::Pointer BSplinesFESpaceLibrary_CreatePrimitiveFESpace3(BSplinesFESpaceLibrary& rDummy, const std::size_t& order_u, const std::size_t& order_v, const std::size_t& order_w)
{
    std::vector<std::size_t> orders(3);
    orders[0] = order_u;
    orders[1] = order_v;
    orders[2] = order_w;
    return rDummy.CreatePrimitiveFESpace<3>(orders);
}

BSplinesFESpace<1>::Pointer BSplinesFESpaceLibrary_CreateUniformFESpace1(BSplinesFESpaceLibrary& rDummy,
    const std::size_t& number_u, const std::size_t& order_u)
{
    std::vector<std::size_t> numbers(1);
    numbers[0] = number_u;
    std::vector<std::size_t> orders(1);
    orders[0] = order_u;
    return rDummy.CreateUniformFESpace<1>(numbers, orders);
}

BSplinesFESpace<2>::Pointer BSplinesFESpaceLibrary_CreateUniformFESpace2(BSplinesFESpaceLibrary& rDummy,
    const std::size_t& number_u, const std::size_t& order_u,
    const std::size_t& number_v, const std::size_t& order_v)
{
    std::vector<std::size_t> numbers(2);
    numbers[0] = number_u;
    numbers[1] = number_v;
    std::vector<std::size_t> orders(2);
    orders[0] = order_u;
    orders[1] = order_v;
    return rDummy.CreateUniformFESpace<2>(numbers, orders);
}

BSplinesFESpace<3>::Pointer BSplinesFESpaceLibrary_CreateUniformFESpace3(BSplinesFESpaceLibrary& rDummy,
    const std::size_t& number_u, const std::size_t& order_u,
    const std::size_t& number_v, const std::size_t& order_v,
    const std::size_t& number_w, const std::size_t& order_w)
{
    std::vector<std::size_t> numbers(3);
    numbers[0] = number_u;
    numbers[1] = number_v;
    numbers[2] = number_w;
    std::vector<std::size_t> orders(3);
    orders[0] = order_u;
    orders[1] = order_v;
    orders[2] = order_w;
    return rDummy.CreateUniformFESpace<3>(numbers, orders);
}

//////////////////////////////////////////////////

void DomainManager2D_AddCell(DomainManager2D& rDummy, const double& x1, const double& x2, const double& y1, const double& y2)
{
    std::vector<double> box(4);
    box[0] = x1;
    box[1] = x2;
    box[2] = y1;
    box[3] = y2;

    rDummy.AddCell(box);
}

bool DomainManager2D_IsInside(DomainManager2D& rDummy, const double& x1, const double& x2, const double& y1, const double& y2)
{
    std::vector<double> box(4);
    box[0] = x1;
    box[1] = x2;
    box[2] = y1;
    box[3] = y2;

    return rDummy.IsInside(box);
}

//////////////////////////////////////////////////

template<int TDim>
void NURBSTestUtils_ProbeAndTestValuesOnPatch(NURBSTestUtils<TDim>& dummy, typename Patch<TDim>::Pointer pPatch,
    ModelPart::ConditionsContainerType& rConditions, const int& integration_order, const double& tol)
{
    GeometryData::IntegrationMethod integration_method = IsogeometricUtility::GetIntegrationMethod(integration_order);
    dummy.ProbeAndTestValuesOnPatch(pPatch, rConditions, integration_method, tol);
}

//////////////////////////////////////////////////

template<int TDim, typename TDataType>
struct StructuredControlGrid_Helper
{
    static boost::python::list GetValue(StructuredControlGrid<TDim, TDataType>& rDummy)
    {
        KRATOS_THROW_ERROR(std::logic_error, __FUNCTION__, "not implemented")
    }

    static void SetValue(StructuredControlGrid<TDim, TDataType>& rDummy, boost::python::list values)
    {
        KRATOS_THROW_ERROR(std::logic_error, __FUNCTION__, "not implemented")
    }
};

template<typename TDataType>
struct StructuredControlGrid_Helper<1, TDataType>
{
    static boost::python::list GetValue(StructuredControlGrid<1, TDataType>& rDummy)
    {
        boost::python::list output;

        for (std::size_t i = 0; i < rDummy.size(); ++i)
        {
            boost::python::list v = ControlValue_Helper<TDataType>::GetValue(rDummy.GetValue(i));
            output.append(v);
        }

        return output;
    }

    static void SetValue(StructuredControlGrid<1, TDataType>& rDummy, boost::python::list values)
    {
        KRATOS_THROW_ERROR(std::logic_error, __FUNCTION__, "not implemented")
    }

    static void SetValue1D(StructuredControlGrid<1, TDataType>& rDummy, const std::size_t& i, const TDataType& value)
    {
        rDummy.SetValue(i, value);
    }

    static TDataType GetValue1D(StructuredControlGrid<1, TDataType>& rDummy, const std::size_t& i)
    {
        return rDummy.GetValue(i);
    }
};

template<typename TDataType>
struct StructuredControlGrid_Helper<2, TDataType>
{
    static boost::python::list GetValue(StructuredControlGrid<2, TDataType>& rDummy)
    {
        boost::python::list output;

        for (std::size_t j = 0; j < rDummy.Size(1); ++j)
        {
            boost::python::list row;
            for (std::size_t i = 0; i < rDummy.Size(0); ++i)
            {
                boost::python::list v = ControlValue_Helper<TDataType>::GetValue(rDummy.GetValue(i, j));
                row.append(v);
            }
            output.append(row);
        }

        return output;
    }

    static void SetValue(StructuredControlGrid<2, TDataType>& rDummy, boost::python::list values)
    {
        KRATOS_THROW_ERROR(std::logic_error, __FUNCTION__, "not implemented")
    }

    static void SetValue2D(StructuredControlGrid<2, TDataType>& rDummy, const std::size_t& i, const std::size_t& j, const TDataType& value)
    {
        rDummy.SetValue(i, j, value);
    }

    static TDataType GetValue2D(StructuredControlGrid<2, TDataType>& rDummy, const std::size_t& i, const std::size_t& j)
    {
        return rDummy.GetValue(i, j);
    }
};

template<typename TDataType>
struct StructuredControlGrid_Helper<3, TDataType>
{
    static boost::python::list GetValue(StructuredControlGrid<3, TDataType>& rDummy)
    {
        boost::python::list output;

        for (std::size_t k = 0; k < rDummy.Size(2); ++k)
        {
            boost::python::list row;
            for (std::size_t j = 0; j < rDummy.Size(1); ++j)
            {
                boost::python::list col;
                for (std::size_t i = 0; i < rDummy.Size(0); ++i)
                {
                    boost::python::list v = ControlValue_Helper<TDataType>::GetValue(rDummy.GetValue(i, j, k));
                    col.append(v);
                }
                row.append(col);
            }
            output.append(row);
        }

        return output;
    }

    static void SetValue(StructuredControlGrid<3, TDataType>& rDummy, boost::python::list values)
    {
        KRATOS_THROW_ERROR(std::logic_error, __FUNCTION__, "not implemented")
    }

    static void SetValue3D(StructuredControlGrid<3, TDataType>& rDummy, const std::size_t& i, const std::size_t& j, const std::size_t& k, const TDataType& value)
    {
        rDummy.SetValue(i, j, k, value);
    }

    static TDataType GetValue3D(StructuredControlGrid<3, TDataType>& rDummy, const std::size_t& i, const std::size_t& j, const std::size_t& k)
    {
        return rDummy.GetValue(i, j, k);
    }
};

void IsogeometricApplication_AddStructuredControlGridsToPython()
{
    /////////////////////////////////////////////////////////////////////////////////////////////////

    class_<BaseStructuredControlGrid<ControlPoint<double> >, BaseStructuredControlGrid<ControlPoint<double> >::Pointer, bases<ControlGrid<ControlPoint<double> > >, boost::noncopyable>
    ("BaseStructuredControlPointGrid", init<>())
    .def(self_ns::str(self))
    ;

    class_<BaseStructuredControlGrid<double>, BaseStructuredControlGrid<double>::Pointer, bases<ControlGrid<double> >, boost::noncopyable>
    ("BaseStructuredDoubleControlGrid", init<>())
    .def(self_ns::str(self))
    ;

    class_<BaseStructuredControlGrid<array_1d<double, 3> >, BaseStructuredControlGrid<array_1d<double, 3> >::Pointer, bases<ControlGrid<array_1d<double, 3> > >, boost::noncopyable>
    ("BaseStructuredArray1DControlGrid", init<>())
    .def(self_ns::str(self))
    ;

    class_<BaseStructuredControlGrid<Vector>, BaseStructuredControlGrid<Vector>::Pointer, bases<ControlGrid<Vector> >, boost::noncopyable>
    ("BaseStructuredVectorControlGrid", init<>())
    .def(self_ns::str(self))
    ;

    /////////////////////////////////////////////////////////////////////////////////////////////////

    class_<StructuredControlGrid<1, ControlPoint<double> >, StructuredControlGrid<1, ControlPoint<double> >::Pointer, bases<BaseStructuredControlGrid<ControlPoint<double> > >, boost::noncopyable>
    ("StructuredControlPointGrid1D", init<const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<1, ControlPoint<double> >::GetValue, &StructuredControlGrid_Helper<1, ControlPoint<double> >::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<1, ControlPoint<double> >::SetValue1D)
    .def("GetValue", &StructuredControlGrid_Helper<1, ControlPoint<double> >::GetValue1D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<1, double>, StructuredControlGrid<1, double>::Pointer, bases<BaseStructuredControlGrid<double> >, boost::noncopyable>
    ("StructuredDoubleControlGrid1D", init<const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<1, double>::GetValue, &StructuredControlGrid_Helper<1, double>::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<1, double>::SetValue1D)
    .def("GetValue", &StructuredControlGrid_Helper<1, double>::GetValue1D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<1, array_1d<double, 3> >, StructuredControlGrid<1, array_1d<double, 3> >::Pointer, bases<BaseStructuredControlGrid<array_1d<double, 3> > >, boost::noncopyable>
    ("StructuredArray1DControlGrid1D", init<const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<1, array_1d<double, 3> >::GetValue, &StructuredControlGrid_Helper<1, array_1d<double, 3> >::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<1, array_1d<double, 3> >::SetValue1D)
    .def("GetValue", &StructuredControlGrid_Helper<1, array_1d<double, 3> >::GetValue1D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<1, Vector>, StructuredControlGrid<1, Vector>::Pointer, bases<BaseStructuredControlGrid<Vector> >, boost::noncopyable>
    ("StructuredVectorControlGrid1D", init<const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<1, Vector>::GetValue, &StructuredControlGrid_Helper<1, Vector>::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<1, Vector>::SetValue1D)
    .def("GetValue", &StructuredControlGrid_Helper<1, Vector>::GetValue1D)
    .def(self_ns::str(self))
    ;

    /////////////////////////////////////////////////////////////////////////////////////////////////

    class_<StructuredControlGrid<2, ControlPoint<double> >, StructuredControlGrid<2, ControlPoint<double> >::Pointer, bases<BaseStructuredControlGrid<ControlPoint<double> > >, boost::noncopyable>
    ("StructuredControlPointGrid2D", init<const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<2, ControlPoint<double> >::GetValue, &StructuredControlGrid_Helper<2, ControlPoint<double> >::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<2, ControlPoint<double> >::SetValue2D)
    .def("GetValue", &StructuredControlGrid_Helper<2, ControlPoint<double> >::GetValue2D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<2, double>, StructuredControlGrid<2, double>::Pointer, bases<BaseStructuredControlGrid<double> >, boost::noncopyable>
    ("StructuredDoubleControlGrid2D", init<const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<2, double>::GetValue, &StructuredControlGrid_Helper<2, double>::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<2, double>::SetValue2D)
    .def("GetValue", &StructuredControlGrid_Helper<2, double>::GetValue2D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<2, array_1d<double, 3> >, StructuredControlGrid<2, array_1d<double, 3> >::Pointer, bases<BaseStructuredControlGrid<array_1d<double, 3> > >, boost::noncopyable>
    ("StructuredArray1DControlGrid2D", init<const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<2, array_1d<double, 3> >::GetValue, &StructuredControlGrid_Helper<2, array_1d<double, 3> >::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<2, array_1d<double, 3> >::SetValue2D)
    .def("GetValue", &StructuredControlGrid_Helper<2, array_1d<double, 3> >::GetValue2D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<2, Vector>, StructuredControlGrid<2, Vector>::Pointer, bases<BaseStructuredControlGrid<Vector> >, boost::noncopyable>
    ("StructuredVectorControlGrid2D", init<const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<2, Vector>::GetValue, &StructuredControlGrid_Helper<2, Vector>::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<2, Vector>::SetValue2D)
    .def("GetValue", &StructuredControlGrid_Helper<2, Vector>::GetValue2D)
    .def(self_ns::str(self))
    ;

    /////////////////////////////////////////////////////////////////////////////////////////////////

    class_<StructuredControlGrid<3, ControlPoint<double> >, StructuredControlGrid<3, ControlPoint<double> >::Pointer, bases<BaseStructuredControlGrid<ControlPoint<double> > >, boost::noncopyable>
    ("StructuredControlPointGrid3D", init<const std::size_t&, const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<3, ControlPoint<double> >::GetValue, &StructuredControlGrid_Helper<3, ControlPoint<double> >::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<3, ControlPoint<double> >::SetValue3D)
    .def("GetValue", &StructuredControlGrid_Helper<3, ControlPoint<double> >::GetValue3D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<3, double>, StructuredControlGrid<3, double>::Pointer, bases<BaseStructuredControlGrid<double> >, boost::noncopyable>
    ("StructuredDoubleControlGrid3D", init<const std::size_t&, const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<3, double>::GetValue, &StructuredControlGrid_Helper<3, double>::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<3, double>::SetValue3D)
    .def("GetValue", &StructuredControlGrid_Helper<3, double>::GetValue3D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<3, array_1d<double, 3> >, StructuredControlGrid<3, array_1d<double, 3> >::Pointer, bases<BaseStructuredControlGrid<array_1d<double, 3> > >, boost::noncopyable>
    ("StructuredArray1DControlGrid3D", init<const std::size_t&, const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<1, array_1d<double, 3> >::GetValue, &StructuredControlGrid_Helper<1, array_1d<double, 3> >::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<3, array_1d<double, 3> >::SetValue3D)
    .def("GetValue", &StructuredControlGrid_Helper<3, array_1d<double, 3> >::GetValue3D)
    .def(self_ns::str(self))
    ;

    class_<StructuredControlGrid<3, Vector>, StructuredControlGrid<3, Vector>::Pointer, bases<BaseStructuredControlGrid<Vector> >, boost::noncopyable>
    ("StructuredVectorControlGrid3D", init<const std::size_t&, const std::size_t&, const std::size_t&>())
    .add_property("ControlValues", &StructuredControlGrid_Helper<3, Vector>::GetValue, &StructuredControlGrid_Helper<3, Vector>::SetValue)
    .def("SetValue", &StructuredControlGrid_Helper<3, Vector>::SetValue3D)
    .def("GetValue", &StructuredControlGrid_Helper<3, Vector>::GetValue3D)
    .def(self_ns::str(self))
    ;

    /////////////////////////////////////////////////////////////////////////////////////////////////
}

//////////////////////////////////////////////////

template<int TDim>
void IsogeometricApplication_AddBSplinesFESpaceToPython()
{
    std::stringstream ss;
    ss.str(std::string());
    ss << "BSplinesFESpace" << TDim << "D";
    class_<BSplinesFESpace<TDim>, typename BSplinesFESpace<TDim>::Pointer, bases<FESpace<TDim> >, boost::noncopyable>
    (ss.str().c_str(), init<>())
    .def("Number", &BSplinesFESpace<TDim>::Number)
    .add_property("KnotU", BSplinesFESpace_GetKnotVector<TDim, 0>, BSplinesFESpace_SetKnotVector<TDim, 0>)
    .add_property("KnotV", BSplinesFESpace_GetKnotVector<TDim, 1>, BSplinesFESpace_SetKnotVector<TDim, 1>)
    .add_property("KnotW", BSplinesFESpace_GetKnotVector<TDim, 2>, BSplinesFESpace_SetKnotVector<TDim, 2>)
    .def(self_ns::str(self))
    ;
}

//////////////////////////////////////////////////

template<int TDim>
void IsogeometricApplication_AddBendingStripNURBSToPython()
{
    std::stringstream ss;

    ss.str(std::string());
    ss << "BendingStripNURBSPatch" << TDim << "D";
    class_<BendingStripNURBSPatch<TDim>, bases<PatchInterface<TDim>, Patch<TDim> > >
    // class_<BendingStripNURBSPatch<TDim>, typename BendingStripNURBSPatch<TDim>::Pointer >
    (ss.str().c_str(), init<const std::size_t&, const int&>())
    .def(init<const std::size_t&, typename Patch<TDim>::Pointer, const BoundarySide&, typename Patch<TDim>::Pointer, const BoundarySide&, const int&>())
    // .def(self_ns::str(self_ns::self))
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "BendingStripNURBSPatch" << TDim << "DPointer";
    class_<typename BendingStripNURBSPatch<TDim>::Pointer>
    (ss.str().c_str(), init<typename BendingStripNURBSPatch<TDim>::Pointer>())
    .def("GetReference", GetReference<BendingStripNURBSPatch<TDim> >, return_value_policy<reference_existing_object>())
    .def(self_ns::str(self))
    ;
}

//////////////////////////////////////////////////

template<int TDim>
void IsogeometricApplication_AddNURBSTestUtilsToPython()
{
    std::stringstream ss;

    ss.str(std::string());
    ss << "NURBSTestUtils" << TDim << "D";
    class_<NURBSTestUtils<TDim>, typename NURBSTestUtils<TDim>::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<>())
    .def("ProbeAndTestValuesOnPatch", &NURBSTestUtils_ProbeAndTestValuesOnPatch<TDim>)
    .def(self_ns::str(self))
    ;
}

//////////////////////////////////////////////////

void IsogeometricApplication_AddNURBSToPython()
{
    /////////////////////////////////////////////////////////////////
    ///////////////////////SUPPORT DOMAIN////////////////////////////
    /////////////////////////////////////////////////////////////////

    class_<DomainManager, DomainManager::Pointer, boost::noncopyable>
    ("DomainManager", init<std::size_t>())
    ;

    class_<DomainManager2D, DomainManager2D::Pointer, boost::noncopyable>
    ("DomainManager2D", init<std::size_t>())
    .def("AddXcoord", &DomainManager2D::AddXcoord)
    .def("AddYcoord", &DomainManager2D::AddYcoord)
    .def("AddCell", &DomainManager2D_AddCell)
    .def("IsInside", &DomainManager2D_IsInside)
    .def(self_ns::str(self))
    ;

    /////////////////////////////////////////////////////////////////
    ///////////////////////CONTROL GRIDS/////////////////////////////
    /////////////////////////////////////////////////////////////////

    IsogeometricApplication_AddStructuredControlGridsToPython();

    /////////////////////////////////////////////////////////////////
    ///////////////////////FESpace///////////////////////////////////
    /////////////////////////////////////////////////////////////////

    IsogeometricApplication_AddBSplinesFESpaceToPython<1>();
    IsogeometricApplication_AddBSplinesFESpaceToPython<2>();
    IsogeometricApplication_AddBSplinesFESpaceToPython<3>();

    class_<BSplinesFESpaceLibrary, BSplinesFESpaceLibrary::Pointer, boost::noncopyable>
    ("BSplinesFESpaceLibrary", init<>())
    .def("CreateLinearFESpace", &BSplinesFESpaceLibrary_CreatePrimitiveFESpace1) // backward compatibility
    .def("CreateRectangularFESpace", &BSplinesFESpaceLibrary_CreatePrimitiveFESpace2) // backward compatibility
    .def("CreateCubicFESpace", &BSplinesFESpaceLibrary_CreatePrimitiveFESpace3) // backward compatibility
    .def("CreatePrimitiveFESpace", &BSplinesFESpaceLibrary_CreatePrimitiveFESpace1)
    .def("CreatePrimitiveFESpace", &BSplinesFESpaceLibrary_CreatePrimitiveFESpace2)
    .def("CreatePrimitiveFESpace", &BSplinesFESpaceLibrary_CreatePrimitiveFESpace3)
    .def("CreateUniformFESpace", &BSplinesFESpaceLibrary_CreateUniformFESpace1)
    .def("CreateUniformFESpace", &BSplinesFESpaceLibrary_CreateUniformFESpace2)
    .def("CreateUniformFESpace", &BSplinesFESpaceLibrary_CreateUniformFESpace3)
    ;

    /////////////////////////////////////////////////////////////////
    ///////////////////////Bending Strip NURBS Patch/////////////////
    /////////////////////////////////////////////////////////////////

    IsogeometricApplication_AddBendingStripNURBSToPython<2>();
    IsogeometricApplication_AddBendingStripNURBSToPython<3>();

    /////////////////////////////////////////////////////////////////
    ///////////////////////NURBS Tets Utils//////////////////////////
    /////////////////////////////////////////////////////////////////

    IsogeometricApplication_AddNURBSTestUtilsToPython<1>();
    // IsogeometricApplication_AddNURBSTestUtilsToPython<2>();
    // IsogeometricApplication_AddNURBSTestUtilsToPython<3>();

}

}  // namespace Python.

} // Namespace Kratos


// This ray-tracing code is adapted from Hugues Thomas' project.
#include <math.h>
#include <iostream>
#include <unordered_map>
#include <algorithm>
#include <set>
#include <cstdint>
#include <cstdio>
#include <boost/algorithm/string.hpp>
#include "ray_tracing.hpp"
#include "estimation.hpp"
#include "utils.hpp"
#include "pointmatcher/PointMatcher.h"

static Eigen::Vector3d max_point(Eigen::MatrixXd & pc) {
    Eigen::Vector3d maxP = Eigen::Vector3d::Zero();
    for (uint i = 0; i < pc.cols(); ++i) {
        if (pc(0, i) > maxP(0))
            maxP(0) = pc(0, i);
        if (pc(1, i) > maxP(1))
            maxP(1) = pc(1, i);
        if (pc(2, i) > maxP(2))
            maxP(2) = pc(2, i);
    }
    return maxP;
}

static Eigen::Vector3d min_point(Eigen::MatrixXd & pc) {
    Eigen::Vector3d minP = Eigen::Vector3d::Zero();
    for (uint i = 0; i < pc.cols(); ++i) {
        if (pc(0, i) < minP(0))
            minP(0) = pc(0, i);
        if (pc(1, i) < minP(1))
            minP(1) = pc(1, i);
        if (pc(2, i) < minP(2))
            minP(2) = pc(2, i);
    }
    return minP;
}

static void cart2pol_(Eigen::MatrixXd &cart_frame, Eigen::MatrixXd &polar_frame) {
    polar_frame = Eigen::MatrixXd::Ones(cart_frame.rows(), cart_frame.cols());
    for (uint i = 0; i < cart_frame.cols(); ++i) {
        Eigen::Vector3d p = cart_frame.block(0, i, 3, 1);
        float rho = p.norm();
        float phi = atan2f(p(1), p(0));
        float theta = atan2f(sqrt(p(0) * p(0) + p(1) * p(1)), p(2));
        polar_frame(0, i) = rho;
        polar_frame(1, i) = theta;
        polar_frame(2, i) = phi + M_PI / 2;
    }
}

static Eigen::Vector3d cart2pol(Eigen::Vector3d &p) {
    float rho = p.norm();
    float phi = atan2f(p(1), p(0));
    float theta = atan2f(sqrt(p(0) * p(0) + p(1) * p(1)), p(2));
    return Eigen::Vector3d(rho, theta, phi + M_PI / 2);
}

static void compare_map_to_frame(Eigen::MatrixXd &aligned_frame, Eigen::MatrixXd &map_points,
    Eigen::MatrixXd &map_normals, Eigen::Matrix4d T_sensor_map, std::unordered_map<VoxKey, size_t> &map_samples,
    float theta_dl, float phi_dl, float map_dl, std::vector<float> &movable_probs, std::vector<int> &movable_counts) {

    float inv_theta_dl = 1.0 / theta_dl;
    float inv_phi_dl = 1.0 / phi_dl;
    float inv_map_dl = 1.0 / map_dl;
    float max_angle = 5 * M_PI / 12;
    float min_vert_cos = cos(M_PI / 3);

    // Mask of the map points not updated yet
    std::vector<bool> not_updated(map_points.cols(), true);

    // Get limits
    Eigen::Vector3d min_P = min_point(aligned_frame) - Eigen::Vector3d(map_dl, map_dl, map_dl);
    Eigen::Vector3d max_P = max_point(aligned_frame) + Eigen::Vector3d(map_dl, map_dl, map_dl);

    // Update full voxels
    // Loop over aligned_frame
    VoxKey k0, k;
    for (uint i = 0; i < aligned_frame.cols(); ++i) {
        // Corresponding key
        k0.x = (int)floor(aligned_frame(0, i) * inv_map_dl);
        k0.y = (int)floor(aligned_frame(1, i) * inv_map_dl);
        k0.z = (int)floor(aligned_frame(2, i) * inv_map_dl);
        // Update the adjacent cells
        for (k.x = k0.x - 1; k.x < k0.x + 2; k.x++) {
            for (k.y = k0.y - 1; k.y < k0.y + 2; k.y++) {
                for (k.z = k0.z - 1; k.z < k0.z + 2; k.z++) {
                    // Update count and movable at this point
                    if (map_samples.count(k) > 0) {
                        // Only update once
                        size_t i0 = map_samples[k];
                        if (not_updated[i0]) {
                            not_updated[i0] = false;
                            movable_counts[i0] += 1;
                        }
                    }
                }
            }
        }
    }

    // Create the free frustum grid

    // get frame in polar coordinates
    Eigen::MatrixXd frame = T_sensor_map * aligned_frame;
    Eigen::MatrixXd polar_frame;
    cart2pol_(frame, polar_frame);

    // Get grid limits
    Eigen::Vector3d minCorner = min_point(polar_frame);
    Eigen::Vector3d maxCorner = max_point(polar_frame);
    Eigen::Vector3d originCorner = minCorner - Eigen::Vector3d(0, 0.5 * theta_dl, 0.5 * phi_dl);

    // Dimensions of the grid
    size_t grid_n_theta = (size_t)floor((maxCorner(1) - originCorner(1)) / theta_dl) + 1;
    size_t grid_n_phi = (size_t)floor((maxCorner(2) - originCorner(2)) / phi_dl) + 1;

    // Initialize variables
    std::vector<float> frustrum_radiuses(grid_n_theta * grid_n_phi, -1.0);

    // Fill the frustrum radiuses
    for (uint i = 0; i < polar_frame.cols(); ++i) {
        // Corresponding key
        Eigen::Vector3d p = polar_frame.block(0, i, 3, 1);
        // Position of point in grid
        size_t i_theta = (size_t)floor((p(1) - originCorner(1)) * inv_theta_dl);
        size_t i_phi = (size_t)floor((p(2) - originCorner(2)) * inv_phi_dl);
        size_t gridIdx = i_theta + grid_n_theta * i_phi;

        // Update the radius in cell
        if (frustrum_radiuses[gridIdx] < 0)
            frustrum_radiuses[gridIdx] = p(0);
        else if (p(0) < frustrum_radiuses[gridIdx])
            frustrum_radiuses[gridIdx] = p(0);
    }

    // Apply margin to free ranges
    float margin = map_dl;
    float frustrum_alpha = theta_dl / 2;
    for (auto &r : frustrum_radiuses) {
        float adapt_margin = r * frustrum_alpha;
        if (margin < adapt_margin)
            r -= adapt_margin;
        else
            r -= margin;
    }

    // Apply frustum casting

    // update free pixels
    float min_r = 2 * map_dl;
    Eigen::Matrix3d C_sensor_map = T_sensor_map.block(0, 0, 3, 3);

    for (uint i = 0; i < map_points.cols(); ++i) {
        // Ignore points updated just now
        if (!not_updated[i])
            continue;
        Eigen::Vector4d p = map_points.block(0, i, 4, 1);
        // Ignore points outside area of the frame
        if (p(0) > max_P(0) || p(1) > max_P(1) || p(2) > max_P(2) ||
            p(0) < min_P(0) || p(1) < min_P(1) || p(2) < min_P(2)) {
            continue;
        }

        // Align point in frame coordinates (and normal)
        p = T_sensor_map * p;
        Eigen::Vector3d xyz = p.block(0, 0, 3, 1);
        Eigen::Vector3d nxyz = C_sensor_map * map_normals.block(0, i, 3, 1);

        // Project in polar coordinates
        Eigen::Vector3d rtp = cart2pol(xyz);

        // Position of point in grid
        size_t i_theta = (size_t)floor((rtp(1) - originCorner(1)) * inv_theta_dl);
        size_t i_phi = (size_t)floor((rtp(2) - originCorner(2)) * inv_phi_dl);
        size_t gridIdx = i_theta + grid_n_theta * i_phi;

        // Update movable prob
        if (rtp(0) > min_r && rtp(0) < frustrum_radiuses[gridIdx]) {
            // Do not update if normal is horizontal and perpendicular to ray (to avoid removing walls)
            if (abs(nxyz(2)) > min_vert_cos) {
                movable_counts[i] += 1;
                movable_probs[i] += 1.0;
            } else {
                float angle = acos(std::min(fabs(xyz.dot(nxyz) / rtp(0)), 1.0));
                if (angle < max_angle) {
                    movable_counts[i] += 1;
                    movable_probs[i] += 1.0;
                }
            }
        }
    }
}

static void getNameFromPath(std::string path, std::string &name) {
    std::vector<std::string> parts;
    boost::split(parts, path, boost::is_any_of("/"));
    std::string endy_bit = parts[parts.size() - 1];
    boost::split(parts, endy_bit, boost::is_any_of("."));
    name = parts[0];
}

int main(int argc, const char *argv[]) {
    std::string root, config;
    std::string new_map_name = "map_no_movable.ply";
    validateArgs(argc, argv, root, config);
    // Params:
    float map_dl = 0.15;
    float theta_dl = 1.29 * M_PI / 180;
    float phi_dl = 0.1 * M_PI / 180;

    // Init point map
    std::cout << "Loading map..." << std::endl;
    DP map = DP::load(root + "map/map.ply");
    std::cout << "Finished loading map" << std::endl;

    // Create the pointmap voxels
    std::unordered_map<VoxKey, size_t> map_samples;
    uint N = map.features.cols();
    map_samples.reserve(N);
    float inv_map_dl = 1.0 / map_dl;
    VoxKey k0;

    for (uint i = 0; i < N; ++i) {
        k0.x = (int)floor(map.features(0, i) * inv_map_dl);
        k0.y = (int)floor(map.features(1, i) * inv_map_dl);
        k0.z = (int)floor(map.features(2, i) * inv_map_dl);
        if (map_samples.count(k0) < 1) {
            map_samples.emplace(k0, i);
        }
    }

    // Init map movable probabilities and counts
    std::vector<float> movable_probs(N, 0);
    std::vector<int> movable_counts(N, 0);

    // Calculate normal vectors for the map
    std::shared_ptr<PM::DataPointsFilter> normalFilter = PM::get().DataPointsFilterRegistrar.create(
        "SurfaceNormalDataPointsFilter", {{"knn", toParam(10)}, {"epsilon", toParam(5)}, {"keepNormals", toParam(1)},
        {"keepDensities", toParam(0)}});

    std::cout << "Calculating normals for the entire map..." << std::endl;
    map = normalFilter->filter(map);
    std::cout << "Finished calculating normals" << std::endl;
    uint norm_row = map.getDescriptorStartingRow("normals");
    Eigen::MatrixXd map_normals = map.descriptors.block(norm_row, 0, 3, N);

    // Start movable detection

    std::vector<std::string> frame_names;
    getMapFrames(root, frame_names);

    for (uint i = 0; i < frame_names.size(); ++i) {
        std::cout << "Ray tracing frame " << i << " / " << frame_names.size() - 1 << std::endl;
        // Load frame / ply file
        DP frame = DP::load(frame_names[i]);
        // Load pose for this frame
        Eigen::Matrix4d T_map_sensor = Eigen::Matrix4d::Identity();
        std::string name;
        getNameFromPath(frame_names[i], name);
        load_transform(root + "map/frame_poses/" + name + ".txt", T_map_sensor);
        Eigen::Matrix4d T_sensor_map = get_inverse_tf(T_map_sensor);
        // Perform ray-tracing to identity movable points
        compare_map_to_frame(frame.features, map.features, map_normals, T_sensor_map,
            map_samples, theta_dl, phi_dl, map_dl, movable_probs, movable_counts);
    }

    for (uint i = 0; i < movable_probs.size(); ++i) {
        movable_probs[i] = movable_probs[i] / (movable_counts[i] + 1e-6);
        if (movable_counts[i] < 1e-6)
            movable_probs[i] = -1;
    }
    map.allocateDescriptor("movable", 1);
    uint movable_row = map.getDescriptorStartingRow("movable");
    for (uint i = 0; i < movable_probs.size(); ++i) {
        map.descriptors(movable_row, i) = movable_probs[i];
    }
    std::cout << "Adding movable descriptor to the existing map..." << std::endl;
    map.removeDescriptor("normals");  // Get rid of normals
    map.save(root + "map/map.ply");

    std::cout << "Removing movable points from the map..." << std::endl;
    uint feat_dim = map.features.rows();
    map.removeDescriptor("movable");
    uint desc_dim = map.descriptors.rows();
    uint j = 0;
    for (uint i = 0; i < movable_probs.size(); ++i) {
        if (movable_probs[i] < 0.8) {
            map.features.block(0, j, feat_dim, 1) = map.features.block(0, i, feat_dim, 1);
            map.descriptors.block(0, j, desc_dim, 1) = map.descriptors.block(0, i, desc_dim, 1);
            j++;
        }
    }
    map.conservativeResize(j);

    std::cout << "Saving the filtered map..." << std::endl;
    map.save(root + "map/" + new_map_name);
}


//
// Created by jachu on 31.01.18.
//

#ifndef PLANELOC_CONCAVEHULL_HPP
#define PLANELOC_CONCAVEHULL_HPP

//#define CGAL_DISABLE_ROUNDING_MATH_CHECK

class ConcaveHull;

#include <vector>

#include <boost/serialization/vector.hpp>

#include <Eigen/Eigen>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/point_cloud.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/algorithm.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/IO/io.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include "Types.hpp"
#include "Serialization.hpp"

class ConcaveHull {

public:
    typedef CGAL::Exact_predicates_exact_constructions_kernel  K;
    typedef K::FT                                                FT;
    typedef K::Point_2                                           Point_2;
    typedef K::Segment_2                                         Segment_2;
    typedef CGAL::Polygon_2<K>                                   Polygon_2;
    typedef CGAL::Polygon_with_holes_2<K>                        Polygon_holes_2;
    typedef CGAL::Polyline_simplification_2::Stop_above_cost_threshold Stop;
    typedef CGAL::Polyline_simplification_2::Squared_distance_cost     Cost;

    typedef CGAL::Exact_predicates_inexact_constructions_kernel    Kie;
    typedef Kie::FT                                                FTie;
    typedef Kie::Point_2                                           Point_2ie;
    typedef Kie::Segment_2                                         Segment_2ie;
    typedef CGAL::Polygon_2<Kie>                                   Polygon_2ie;
    typedef CGAL::Polygon_with_holes_2<Kie>                        Polygon_holes_2ie;
    typedef CGAL::Alpha_shape_vertex_base_2<Kie>                   Vb;
    typedef CGAL::Alpha_shape_face_base_2<Kie>                     Fb;
    typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
    typedef CGAL::Delaunay_triangulation_2<Kie,Tds>                Triangulation_2;
    typedef CGAL::Alpha_shape_2<Triangulation_2>                 Alpha_shape_2;
    typedef Alpha_shape_2::Alpha_shape_edges_iterator            Alpha_shape_edges_iterator;
    typedef Alpha_shape_2::Alpha_shape_vertices_iterator         Alpha_shape_vertices_iterator;

    ConcaveHull();

    ConcaveHull(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr ipoints3d,
                const Eigen::Vector4d &planeEq);

    ConcaveHull(const std::vector<Polygon_2> &polygons,
                    const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &polygons3d,
                    const Eigen::Vector3d &plNormal,
                    double plD,
                    const Eigen::Vector3d &origin,
                    const Eigen::Vector3d &xAxis,
                    const Eigen::Vector3d &yAxis);

    ConcaveHull(const ConcaveHull &other);

    void init(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr ipoints3d,
              const Eigen::Vector4d &planeEq);

    void init(const std::vector<Polygon_2> &polygons,
              const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &polygons3d,
              const Eigen::Vector3d &plNormal,
              double plD,
              const Eigen::Vector3d &origin,
              const Eigen::Vector3d &xAxis,
              const Eigen::Vector3d &yAxis);

    const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &getPolygons3d() const {
        return polygons3d;
    }

    const std::vector<double> &getAreas() const {
        return areas;
    }

    double getTotalArea() const {
        return totalArea;
    }

    ConcaveHull intersect(const ConcaveHull &other,
                          double areaThresh = 0.05) const;

    ConcaveHull intersect(const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &otherPolygons3d,
                          double areaThresh = 0.05) const;

    ConcaveHull clipToCameraFrustum(const cv::Mat K,
                                    int rows,
                                    int cols,
                                    double minZ);

    ConcaveHull transform(const Vector7d &transform) const;

    double minDistance(const ConcaveHull &other) const;

    void display(pcl::visualization::PCLVisualizer::Ptr viewer,
                 int vp,
                 double r = 0.0,
                 double g = 1.0,
                 double b = 0.0) const ;

    void cleanDisplay(pcl::visualization::PCLVisualizer::Ptr viewer,
                      int vp) const;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    void computeFrame();

    Point_2 point3dTo2d(const Eigen::Vector3d &point3d) const;

    Point_2ie point3dTo2die(const Eigen::Vector3d &point3d) const;

    Eigen::Vector3d point2dTo3d(const Point_2 &point2d) const;

    std::vector<Polygon_2> polygons;
    std::vector<double> areas;
    double totalArea;
    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> polygons3d;

    Eigen::Vector3d plNormal;
    double plD;
    // point on the plane nearest to origin
    Eigen::Vector3d origin;
    Eigen::Vector3d xAxis, yAxis;

    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & polygons;
        ar & areas;
        ar & totalArea;
        ar & polygons3d;
        ar & plNormal;
        ar & plD;
        ar & origin;
        ar & xAxis;
        ar & yAxis;
    }
};

#endif //PLANELOC_CONCAVEHULL_HPP


//==================================================================================================
/*!
  @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_TWO_ADD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TWO_ADD_HPP_INCLUDED

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

  /*!

    @ingroup group-arithmetic
    This function object computes two reals of the type of the inputs
    (in an std::pair) @c r0 and @c r1 such that:

    @code
    r0 = x + y
    r1 = r0 -(x + y)
    @endcode

    using perfect arithmetic.

    Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

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

    @par Example:

      @snippet two_add.cpp two_add

    @par Possible output:

      @snippet two_add.txt two_add

  **/
  std::pair<Value, Value> two_add(Value const& x, Value const& y);
} }
#endif

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

#endif


#pragma once
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <vector>

using namespace Eigen;

template <class Type>
Matrix<Type, Dynamic, Dynamic, RowMajor>* load_img(const std::string& path, int num_channels, int img_length) {
    using Mat = Matrix<Type, Dynamic, Dynamic, RowMajor>;

    Mat* img = new Mat[num_channels];

    int cellno = 0, c = 0;
    int img_size = img_length * img_length;

    std::ifstream in(path);
    std::string line;
    std::vector<Type> values(img_size);
    while (std::getline(in, line)) {
        std::stringstream ss(line);
        std::string cell;
        while (std::getline(ss, cell, ',')) {
            Type val = Type(std::stod(cell));
            values[cellno++] = val;

            if (cellno == img_size) {
                cellno = 0;
                img[c] = Map<Mat>(values.data(), img_length, img_length);
                values.clear();
                ++c;
            }
        }
    }

    return img;
}

template <class Type>
Matrix<Type, Dynamic, Dynamic, RowMajor>** load_conv_weights(const std::string& path, int num_channels, int num_kernels, int kernel_length) {
    using Mat = Matrix<Type, Dynamic, Dynamic, RowMajor>;

    Mat** kernels = new Mat*[num_kernels];
    for (int k = 0; k < num_kernels; ++k) {
        kernels[k] = new Mat[num_channels];
    }

    int cellno = 0, c = 0, k = 0;
    int kernel_size = kernel_length * kernel_length;

    std::ifstream in(path);
    std::string line;
    std::vector<Type> values;
    while (std::getline(in, line)) {
        std::stringstream ss(line);
        std::string cell;
        while (std::getline(ss, cell, ',')) {
            Type val = Type(std::stod(cell));
            values.push_back(val);
            ++cellno;

            if (cellno == kernel_size) {
                cellno = 0;
                kernels[k][c] = Map<Mat>(values.data(), kernel_length, kernel_length);
                values.clear();
                ++c;
                if (c == num_channels) {
                    c = 0;
                    ++k;
                }
            }
        }
    }
    return kernels;
}

template <class Type>
Matrix<Type, 1, Dynamic, RowMajor> load_conv_biases(const std::string& path, int num_kernels) {
    using RowVec = Matrix<Type, 1, Dynamic, RowMajor>;

    std::ifstream in(path);
    std::string cell;
    std::vector<Type> values;
    while (std::getline(in, cell, ',')) {
        Type val = Type(std::stod(cell));
        values.push_back(val);
    }
    return Map<RowVec>(values.data(), 1, num_kernels);
}

template <class Type>
Matrix<Type, Dynamic, Dynamic, RowMajor> load_fc(const std::string& path, int M, int N) {
    std::ifstream indata(path);
    std::string line;
    std::vector<Type> values;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ',')) {
            Type val = Type(std::stod(cell));
            values.push_back(val);
        }
    }
    return Map<Matrix<Type, Dynamic, Dynamic, RowMajor>>(values.data(), M, N);
}


#include <iostream>
#include <vector>

#include <dlib/svm.h>
#include <dlib/statistics.h>
#include <dlib/global_optimization.h>

#include <sampml/data.hpp>
#include <sampml/random_forest.hpp>

#include "common.hpp"
#include "transform.hpp"

using sample_type = output_vector;

void train() {
    std::cout << "TRAINING:\n";

    sampml::data::reader<sample_type> data_positive_train(positive_train_data);
    sampml::data::reader<sample_type> data_negative_train(negative_train_data);

    sampml::trainer::random_forest<sample_type> model;
    model.set_samples(data_positive_train, data_negative_train);
    //model.cross_validate();
    model.train();

    model.serialize(std::string(rf_model.begin(), rf_model.end()));
    std::cout << "\n\n";
}

void test() {
    const float cutoff = 0.5;

    std::cout << "TESTING:\n";
    sampml::data::reader<sample_type> data_positive_test(positive_test_data);
    sampml::data::reader<sample_type> data_negative_test(negative_test_data);

    sampml::trainer::random_forest<sample_type> model;
    model.deserialize(std::string(rf_model.begin(), rf_model.end()));

    int true_positives = 0,
        false_positives = 0,
        true_negatives = 0,
        false_negatives = 0;
    dlib::running_stats<double> values;
    for(const auto& v : data_positive_test) {
        double prob = model.test(v);
        if(prob > cutoff)
            true_positives++;
        else
            false_negatives++;
        values.add(prob);
    }
    std::cout << "positive test set statistics: " << '\n';
    std::cout << "average: " << values.mean() << ", stddev: " << values.stddev() << '\n'
              << "min: " << values.min() << ", max: " << values.max()  << '\n'
              << "skewness: " << values.skewness() << ", excess kurtosis: " << values.ex_kurtosis() << '\n';
    values.clear();

    for(const auto& v : data_negative_test) {
        double prob = model.test(v);
        if(prob < cutoff)
            true_negatives++;
        else
            false_positives++;
        values.add(prob);
    }
    std::cout << "negative test set statistics: " << '\n';
    std::cout << "average: " << values.mean() << ", stddev: " << values.stddev() << '\n'
              << "min: " << values.min() << ", max: " << values.max()  << '\n'
              << "skewness: " << values.skewness() << ", excess kurtosis: " << values.ex_kurtosis() << '\n';

    int num_correct = true_positives + true_negatives,
        num_wrong = false_positives + false_negatives;
    std::cout << "\n\n";
    std::cout << "true positives: " << true_positives << ", false positives: " << false_positives << '\n';
    std::cout << "true negatives: " << true_negatives << ", false negatives: " << false_negatives << '\n';
    std::cout << "number of samples classified corretly: " << num_correct << '\n';
    std::cout << "number of samples classified incorrectly: " << num_wrong << '\n';
    std::cout << "accuracy: " << num_correct/float(num_correct + num_wrong) << '\n';
}

int main () {
    train();
    test();
    return 0;
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Cache.h"

using namespace Scine::Molassembler;

unsigned int ackermann(unsigned int m, unsigned int n) {
  if (m == 0) {
    return n + 1;
  }
  if (n == 0) {
    return ackermann(m - 1, 1);
  }
  return ackermann(m - 1, ackermann(m, n - 1));
}

/* Sample class with mutable Cache member and a generatable cache object
 * Also includes an example of how to modify a cache object
 */
class Foo {
private:
  /* 2 */
  mutable Temple::Cache<std::string> cache_ {
    std::make_pair(
      "bigNumber",
      [this]() {
        return (this -> determineMe_());
      }
    )
  };

  unsigned determineMe_() {
    return ackermann(3, 2);
  }

public:
  unsigned getAckermann() const {
    /* 4 */
    return cache_.getGeneratable<unsigned>("bigNumber");
  }

  void changeCacheValue() {
    /* 5 */
    cache_.changeGeneratable<unsigned>(
      "bigNumber",
      [](unsigned* value) {
        *value = 4;
      }
    );
  }
};

BOOST_AUTO_TEST_CASE(SimpleCacheTest, *boost::unit_test::label("Temple")) {
  using namespace std::string_literals;

  /* 1 */
  Temple::Cache<std::string> cache;

  /* 3 */
  std::vector<std::string> keys {"number", "string", "vector"};
  cache.add("number", 5);
  cache.add("string", "fsldkf"s);
  cache.add("vector", std::vector<unsigned>({4, 9}));

  /* 8 */
  if(auto number = cache.getOption<int>("number")) { // op (bool) is true
    BOOST_CHECK(number.value() == 5);
  }

  if(auto number = cache.getOption<int>("non-existent number")) { // op (bool) is false
    // this should not be executed
    BOOST_REQUIRE(false);
  }

  /* 9 */
  BOOST_CHECK(
    std::all_of(
      keys.begin(),
      keys.end(),
      [&cache](const std::string& key) {
        return cache.has(key);
      }
    )
  );

  /* 6 */
  cache.invalidate("number");
  BOOST_CHECK(!cache.has("number"));

  /* 7 */
  cache.invalidate();
  BOOST_CHECK(
    std::none_of(
      keys.begin(),
      keys.end(),
      [&cache](const std::string& key) {
        return cache.has(key);
      }
    )
  );

  /* 2, 4, 5 */
  Foo bar;
  bar.getAckermann();

  // test modification of the cache
  bar.changeCacheValue();
  BOOST_CHECK(bar.getAckermann() == 4);
}