#ifdef COMPILATION// -*-indent-tabs-mode:t;c-basic-offset:4;tab-width:4-*-
$CXX -DNDEBUG $0 -o $0x -lboost_timer&&$0x&&rm $0x;exit
#endif
// © Alfredo A. Correa 2019-2020

#include "../array.hpp"

#include<iostream>
#include<vector>
#include<numeric> // iota
#include<algorithm>

namespace multi = boost::multi;
using std::cout;

template<class Matrix, class Vector, class idx = typename std::decay_t<Vector>::difference_type>
auto gj_solve(Matrix&& A, Vector&& y)->decltype(y[0]/=A[0][0], y){
	idx Asize = size(A);
	for(idx r = 0; r != Asize; ++r){
		auto&& Ar = A[r]; auto const& Arr = Ar[r];
		for(idx c = r + 1; c != Asize; ++c) Ar[c] /= Arr;
		auto const& yr = (y[r] /= Arr);
		for(idx r2 = r + 1; r2 != Asize; ++r2){
			auto&& Ar2 = A[r2]; auto const& Ar2r = A[r2][r];
			for(idx c = r + 1; c != Asize; ++c) Ar2[c] -= Ar2r*Ar[c];
			y[r2] -= Ar2r*yr;
		}
	}
	for(idx r = Asize - 1; r > 0; --r){
		auto const& yr = y[r];
		for(idx r2 = r-1; r2 >=0; --r2) y[r2] -= A[r2][r]*yr;
	}
	return y;
}

template<class Matrix, class Vector, class idx = typename std::decay_t<Vector>::difference_type>
auto gj_solve2(Matrix&& A, Vector&& y)->decltype(y[0]/=A[0][0], y){
	idx Asize = size(A);
	for(idx r = 0; r != Asize; ++r){
		auto&& Ar = A[r]; auto const& Arr = Ar[r];
	//	std::transform(Ar.begin() + r + 1, Ar.end(), Ar.begin() + r + 1, [&](auto const& a){return a/Arr;});
		for(idx c = r + 1; c != Asize; ++c) Ar[c] /= Arr;
		auto const& yr = (y[r] /= Arr);
		for(idx r2 = r + 1; r2 != Asize; ++r2){
			auto&& Ar2 = A[r2]; auto const& Ar2r = A[r2][r];
			std::transform(std::move(Ar2).begin() + r + 1, std::move(Ar2).end(), std::move(Ar).begin() + r + 1, std::move(Ar2).begin() + r + 1, [&](auto&& a, auto&& b){return a - Ar2r*b;});
			y[r2] -= Ar2r*yr;
		}
	}
	for(idx r = Asize - 1; r > 0; --r){
		auto const& yr = y[r];
		for(idx r2 = r-1; r2 >=0; --r2) y[r2] -= A[r2][r]*yr;
	}
	return y;
}

#include <boost/timer/timer.hpp>

int main(){
	{
		multi::array<double, 2> A = {{-3., 2., -4.},{0., 1., 2.},{2., 4., 5.}};
		multi::array<double, 1> y = {12.,5.,2.}; //(M); assert(y.size() == M); iota(y.begin(), y.end(), 3.1);
		gj_solve(A, y);
		cout << y[0] <<" "<< y[1] <<" "<< y[2] << std::endl;
	}
	{
		multi::array<double, 2> A({6000, 7000}); std::iota(A.data(), A.data() + A.num_elements(), 0.1);
		std::transform(A.data(), A.data() + A.num_elements(), A.data(), [](auto x){return x/=2.e6;});
		std::vector<double> y(3000); std::iota(y.begin(), y.end(), 0.2);
		{
			boost::timer::auto_cpu_timer t;
			gj_solve(A({1000, 4000}, {0, 3000}), y);
		}
		cout << y[45] << std::endl;
	}
	{
		multi::array<double, 2> A({6000, 7000}); std::iota(A.data(), A.data() + A.num_elements(), 0.1);
		std::transform(A.data(), A.data() + A.num_elements(), A.data(), [](auto x){return x/=2.e6;});
		std::vector<double> y(3000); std::iota(y.begin(), y.end(), 0.2);
		{
			boost::timer::auto_cpu_timer t;
			gj_solve2(A({1000, 4000}, {0, 3000}), y);
		}
		cout << y[45] << std::endl;
	}
}


// ratslam headers
#include <Ratslam.hpp>
#include <RatslamGraphics.h>

// boost property trees
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

// opencv for video input.
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <string>
#include <iostream>
#include <fstream>
#include <sstream>

double time_diff_to_double(unsigned long sec2, unsigned long nsec2, unsigned long sec1, unsigned long nsec1)
{
	double rsec, rnsec;

	rsec = sec2 - sec1;

	if (nsec2 > nsec1)
	{
		rnsec = nsec2 - nsec1;
	}
	else
	{
		rnsec = 1000000000 - nsec1 + nsec2;
		rsec--;
	}

	return (double)rsec + 1.0/1000000000.0*(double)rnsec;
}

int main(int argc, char * argv[])
{
	if (argc < 2)
	{
		std::cout << argv[0] << "(ERROR): Usage: ratslam config_file.txt" << std::endl;
		cin.get();
		return -1;
	}

	// read settings from the config file
	boost::property_tree::ptree settings;
	read_ini(argv[1], settings);

	// video reader for receiving camera input
	cv::VideoCapture video_reader;
	std::string dataset_video_filename;
	std::string dataset_csv_filename;
	std::string csv_line;
	std::istringstream iss;

	// read the video filename from the settings
	gri::get_setting_from_ptree<std::string>(dataset_video_filename, settings, "gri.robot_name", "");
	dataset_video_filename = "log_" + dataset_video_filename + ".avi";

	// read the csv filename from the settings
	gri::get_setting_from_ptree<std::string>(dataset_csv_filename, settings, "gri.robot_name", "");
	dataset_csv_filename = "log_" + dataset_csv_filename + ".txt";

	video_reader.open(dataset_video_filename);
	if (!video_reader.isOpened())
	{
		std::cout << argv[0] << "(ERROR): Failed to open video " <<
			dataset_video_filename << std::endl;
		cin.get();
		return -1;
	}

	// open the robot's log as a csv to get translational and rotational
	// velocities, and time differences
	std::ifstream csv_in(dataset_csv_filename.c_str());
	if (!csv_in.is_open())
	{
		std::cout << argv[0] << "(ERROR): Failed to open csv file " <<
			dataset_csv_filename << std::endl;
		cin.get();
		return -1;
	}

	// discard the titles line
	getline(csv_in, csv_line);

	// fetch the ratslam settings and construct a new ratslam object
	boost::property_tree::ptree ratslam_settings;
	gri::get_setting_child(ratslam_settings, settings, "ratslam");
	ratslam::Ratslam * ratslam = new ratslam::Ratslam(ratslam_settings);

	// ratslam inputs
	cv::Mat frame;
	double delta_time_s, trans_vel, rot_vel;
	double time_s, last_time_s;
	unsigned long current_sec, current_nsec, last_sec, last_nsec;

	// construct a new ratslam graphics object to allow drawing ratslam
	boost::property_tree::ptree ratslam_graphics_settings;
	gri::get_setting_child(ratslam_graphics_settings, settings, "draw");
	IrrEventReceiver Event_Receiver;
	ratslam::RatslamGraphics * ratslam_graphics =
		new ratslam::RatslamGraphics(ratslam_graphics_settings,
		&Event_Receiver, ratslam);

	//discard one frame
	video_reader.grab();

	// get the associated csv information
	getline(csv_in, csv_line, ',');
	iss.clear();
	iss.str(csv_line);
	iss >> last_sec;
	getline(csv_in, csv_line, ',');
	iss.clear();
	iss.str(csv_line);
	iss >> last_nsec;

	getline(csv_in, csv_line, '\n');

	// start a processing loop
	while (video_reader.grab() && ratslam_graphics->begin())
	{
		// get a frame from the video file
		video_reader.retrieve(frame, 0);

		// get the associated csv information
		getline(csv_in, csv_line, ',');
		iss.clear();
		iss.str(csv_line);
		iss >> current_sec;
		getline(csv_in, csv_line, ',');
		iss.clear();
		iss.str(csv_line);
		iss >> current_nsec;

		delta_time_s = time_diff_to_double(current_sec, current_nsec, last_sec, last_nsec);
		last_sec = current_sec;
		last_nsec = current_nsec;

		getline(csv_in, csv_line, ',');
		iss.clear();
		iss.str(csv_line);
		iss >> trans_vel;
		getline(csv_in, csv_line, ',');
		iss.clear();
		iss.str(csv_line);
		iss >> rot_vel;

		getline(csv_in, csv_line, '\n');

		// set the ratslam inputs
		ratslam->set_view_rgb(frame.data);
		ratslam->set_odom(trans_vel, rot_vel);
		ratslam->set_delta_time(delta_time_s);

		// process a single frame
		ratslam->process();

		ratslam_graphics->end();

	}

	delete ratslam_graphics;
	delete ratslam;

	return 0;
}


#ifndef KIDONO_FEATURE_EXTRACTOR_HPP
#define KIDONO_FEATURE_EXTRACTOR_HPP

#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <boost/range/algorithm.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/impl/plane_clipper3D.hpp>

namespace hdl_people_detection {

/**
 * @brief A class to extract Kidono's features
 * @see Kiyosumi Kidono et al., "Pedestrian Recognition Using High-definition LIDAR"
 * @see http://www.aisl.cs.tut.ac.jp/~jun/pdffiles/kidono-iv2011.pdf
 */
class KidonoFeatureExtractor {
private:
  double distance_scale;
public:
  /**
   * @brief constructor
   * @param distance_scale
   */
  KidonoFeatureExtractor(double distance_scale = 1.0)
    : distance_scale(distance_scale)
  {}

  /**
   * @brief extract features
   * @param cloud  src cloud
   * @return extracted feature vector
   */
  std::vector<float> extract(pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud) const {
    std::vector<float> feature;		feature.reserve(256);
    feature.push_back(cloud->points.size());
    feature.push_back(minimumDistance(cloud) * distance_scale);

    Eigen::Vector3f mean;
    Eigen::Matrix3f covariance;

    boost::copy(covariance3d(cloud, mean, covariance), std::back_inserter(feature));

    auto centered_cloud = centeredCloud(cloud, mean);

    boost::copy(inertiaMoment3d(cloud), std::back_inserter(feature));

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(covariance);

    Eigen::Vector3f e1 = solver.eigenvectors().col(2);
    Eigen::Vector3f e2 = solver.eigenvectors().col(1);
    Eigen::Vector3f e3 = solver.eigenvectors().col(0);

    if (e1.z() < 0.0f) {
      e1 = -e1;
    }

    boost::copy(covarianceIn3zones(centered_cloud, e1, e2, e3), std::back_inserter(feature));
    boost::copy(histogram2d(centered_cloud, e1, e2, 14, 7), std::back_inserter(feature));
    boost::copy(histogram2d(centered_cloud, e1, e3, 9, 5), std::back_inserter(feature));
    boost::copy(sliceFeature(centered_cloud, e1, e2, e3, 10), std::back_inserter(feature));
    boost::copy(intensityDistribution(centered_cloud, 25), std::back_inserter(feature));

    return feature;
  }

private:
  template<typename T>
  T square(T v) const { return v * v; }

  pcl::PointCloud<pcl::PointXYZI>::Ptr centeredCloud(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, const Eigen::Vector3f& mean) const {
    pcl::PointCloud<pcl::PointXYZI>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    centered_cloud->resize(cloud->size());
    for (int i = 0; i < cloud->size(); i++) {
      centered_cloud->at(i).getVector3fMap() = cloud->at(i).getVector3fMap() - mean;
      centered_cloud->at(i).intensity = cloud->at(i).intensity;
    }
    centered_cloud->width = centered_cloud->size();
    centered_cloud->height = 1;
    centered_cloud->is_dense = false;

    return centered_cloud;
  }

  float minimumDistance(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud) const {
    float dist = cloud->front().getVector3fMap().squaredNorm();
    for (int i = 1; i < cloud->size(); i++) {
      dist = std::min(dist, cloud->at(i).getVector3fMap().squaredNorm());
    }
    return sqrtf(dist);
  }

  std::vector<float> covariance3d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, Eigen::Vector3f& mean, Eigen::Matrix3f& covariance) const {
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud, centroid);
    pcl::computeCovarianceMatrix(*cloud, centroid, covariance);

    mean = centroid.topLeftCorner(3, 1);

    std::vector<float> feature(6);
    feature[0] = covariance(0, 0);		feature[1] = covariance(0, 1);		feature[2] = covariance(0, 2);
    feature[3] = covariance(1, 1);		feature[4] = covariance(1, 2);
    feature[5] = covariance(2, 2);
    return feature;
  }

  std::vector<float> inertiaMoment3d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud) const {
    std::vector<float> feature(6, 0.0f);

    for (int i = 0; i < centered_cloud->size(); i++) {
      const auto& pt = centered_cloud->at(i);
      feature[0] += square(pt.y) + square(pt.z);
      feature[1] += -pt.x * pt.y;
      feature[2] += -pt.x * pt.z;
      feature[3] += square(pt.x) + square(pt.z);
      feature[4] += -pt.y * pt.z;
      feature[5] += square(pt.x) + square(pt.y);
    }

    return feature;
  }

  std::vector<float> covariance2d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, const Eigen::Vector3f& e1, const Eigen::Vector3f& e2) const {
    if (cloud->empty()){
      return std::vector<float>(3, 0.0f);
    }

    Eigen::MatrixXf ptdata(cloud->size(), 2);
    for (int i = 0; i < cloud->size(); i++) {
      ptdata(i, 0) = cloud->at(i).getVector3fMap().dot(e1);
      ptdata(i, 1) = cloud->at(i).getVector3fMap().dot(e2);
    }

    Eigen::Vector2f mean = ptdata.colwise().mean().transpose();
    Eigen::MatrixXf centered = ptdata.rowwise() - mean.transpose();
    Eigen::Matrix2f covariance = (centered.transpose() * centered) / cloud->size();

    std::vector<float> feature(3);
    feature[0] = covariance(0, 0);	feature[1] = covariance(0, 1);
    feature[2] = covariance(1, 1);

    return feature;
  }

  std::vector<float> covarianceIn3zones(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud, const Eigen::Vector3f& e1, const Eigen::Vector3f& e2, const Eigen::Vector3f& e3) const {
    pcl::PointCloud<pcl::PointXYZI>::Ptr upper_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr lower_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr right_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr left_cloud(new pcl::PointCloud<pcl::PointXYZI>());

    pcl::PointIndices::Ptr indices(new pcl::PointIndices());

    pcl::PlaneClipper3D<pcl::PointXYZI> clipper(Eigen::Vector4f(e1.x(), e1.y(), e1.z(), 0.0f));
    clipper.clipPointCloud3D(*centered_cloud, indices->indices);

    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(centered_cloud);
    extract.setIndices(indices);
    extract.filter(*upper_cloud);

    extract.setNegative(true);
    extract.filter(*lower_cloud);

    indices->indices.clear();
    clipper.setPlaneParameters(Eigen::Vector4f(e2.x(), e2.y(), e2.z(), 0.0f));
    clipper.clipPointCloud3D(*lower_cloud, indices->indices);

    extract.setInputCloud(lower_cloud);
    extract.setIndices(indices);
    extract.setNegative(false);
    extract.filter(*left_cloud);

    extract.setNegative(true);
    extract.filter(*right_cloud);

    std::vector<float> feature;		feature.reserve(9);
    boost::copy(covariance2d(upper_cloud, e1, e2), std::back_inserter(feature));
    boost::copy(covariance2d(left_cloud, e1, e2), std::back_inserter(feature));
    boost::copy(covariance2d(right_cloud, e1, e2), std::back_inserter(feature));

    return feature;
  }

  std::vector<float> histogram2d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, const Eigen::Vector3f& axis1, const Eigen::Vector3f& axis2, int bin1, int bin2) const {
    std::vector<Eigen::Vector2f> pts2d(cloud->size());
    for (int i = 0; i < cloud->size(); i++) {
      pts2d[i][0] = cloud->at(i).getVector3fMap().dot(axis1);
      pts2d[i][1] = cloud->at(i).getVector3fMap().dot(axis2);
    }

    Eigen::Array2f min_pt = pts2d.front();
    Eigen::Array2f max_pt = pts2d.front();
    for (int i = 1; i < pts2d.size(); i++) {
      min_pt = pts2d[i].array().min(min_pt);
      max_pt = pts2d[i].array().max(max_pt);
    }

    Eigen::Array2f size = max_pt - min_pt;
    Eigen::Array2f inv_size = Eigen::Array2f(bin1 - 0.1f, bin2 - 0.1f) / size;

    float weight = 1.0f / pts2d.size();
    std::vector<float> hist(bin1 * bin2, 0.0f);
    for (int i = 0; i < pts2d.size(); i++) {
      Eigen::Array2i index = ((pts2d[i].array() - min_pt) * inv_size).cast<int>();
      hist[index[0] + index[1] * bin1] += weight;
    }

    return hist;
  }

  std::vector<float> sliceFeature(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud, const Eigen::Vector3f& e1, const Eigen::Vector3f& e2, const Eigen::Vector3f& e3, int slice_n = 10) const {
    Eigen::Matrix3f cvt2eigenspace;
    cvt2eigenspace << e1.transpose(), e2.transpose(), e3.transpose();

    float e1_min = 9999.0f;
    float e1_max = -9999.0f;

    std::vector<Eigen::Vector3f> aligned_cloud(centered_cloud->size());
    for (int i = 0; i < centered_cloud->size(); i++) {
      aligned_cloud[i] = cvt2eigenspace * centered_cloud->at(i).getVector3fMap();
      e1_min = std::min(e1_min, aligned_cloud[i][0]);
      e1_max = std::max(e1_max, aligned_cloud[i][0]);
    }

    float height = e1_max - e1_min;
    float scale = (slice_n - 0.1f) / height;

    std::vector<Eigen::Array2f> min_pts(slice_n);
    std::vector<Eigen::Array2f> max_pts(slice_n);
    for (int i = 0; i < slice_n; i++) {
      min_pts[i] = Eigen::Array2f::Ones() * 9999.0f;
      max_pts[i] = Eigen::Array2f::Ones() * -9999.0f;
    }

    for (int i = 0; i < aligned_cloud.size(); i++) {
      int n = static_cast<int>((aligned_cloud[i][0] - e1_min) * scale);
      min_pts[n] = min_pts[n].min(aligned_cloud[i].bottomLeftCorner(2, 1).array());
      max_pts[n] = max_pts[n].max(aligned_cloud[i].bottomLeftCorner(2, 1).array());
    }

    std::vector<float> feature;
    feature.reserve(slice_n * 2);

    for (int i = 0; i < slice_n; i++){
      Eigen::Array2f w = max_pts[i] - min_pts[i];
      if (w[0] > 0.0f && w[1] > 0.0f) {
        feature.push_back(w[0]);
        feature.push_back(w[1]);
      }
      else {
        feature.push_back(0.0f);
        feature.push_back(0.0f);
      }
    }

    return feature;
  }

  std::vector<float> intensityDistribution(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud, int hist_n = 25) const {
    Eigen::ArrayXf intensity(centered_cloud->size());
    for (int i = 0; i < centered_cloud->size(); i++) {
      intensity[i] = centered_cloud->at(i).intensity;
    }

    float weight = 1.0f / intensity.size();
    std::vector<float> feature(hist_n + 2, 0.0f);

    float scale = (hist_n - 0.1f) / 255.0f;
    for (int i = 0; i < intensity.size(); i++) {
      int n = static_cast<int>(intensity[i] * scale);
      feature[n] += weight;
    }

    float mean = intensity.mean();
    float stddev = sqrt((intensity - mean).square().mean());

    feature[hist_n] = mean;
    feature[hist_n + 1] = stddev;

    return feature;
  }
};

}

#endif


//
//  Copyright (c) 2007
//  Tsai, Dung-Bang
//  National Taiwan University, Department of Physics
//
//  E-Mail : dbtsai (at) gmail.com
//  Begine : 2007/11/20
//  Last modify : 2007/11/22
//  Version : v0.1
//
//  EXPGM_PAD computes the matrix exponential exp(H) for general matrixs,
//  including complex and real matrixs using the irreducible (p,p) degree
//  rational Pade approximation to the exponential
//  exp(z) = r(z)=(+/-)( I+2*(Q(z)/P(z))).
//
//  Usage :
//
//    U = expm_pad(H)
//    U = expm_pad(H, p)
//
//    where p is internally set to 6 (recommended and gererally satisfactory).
//
//  See also MATLAB supplied functions, EXPM and EXPM1.
//
//  Reference :
//  EXPOKIT, Software Package for Computing Matrix Exponentials.
//  ACM - Transactions On Mathematical Software, 24(1):130-156, 1998
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_EXPM_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_EXPM_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <complex>
#include <stdexcept>

namespace boost { namespace numeric { namespace ublasx {

template<typename MATRIX>
MATRIX expm_pad(const MATRIX &H, const int p = 6)
{
	typedef typename MATRIX::value_type value_type;
        typedef typename MATRIX::size_type size_type;
	typedef double real_value_type;	// Correct me. Need to modify.
	assert(H.size1() == H.size2());
	const size_type n = H.size1();
	const identity_matrix<value_type> I(n);
	matrix<value_type> U(n,n),H2(n,n),P(n,n),Q(n,n);
	real_value_type norm = 0.0;

// Calcuate Pade coefficients  (1-based instead of 0-based as in the c vector)
	vector<real_value_type> c(p+2);
	c(1)=1;
	for(size_type i = 1; i <= p; ++i)
		c(i+1) = c(i) * ((p + 1.0 - i)/(i * (2.0 * p + 1 - i)));
// Calcuate the infinty norm of H, which is defined as the largest row sum of a matrix
	for(size_type i=0; i<n; ++i)
	{
		real_value_type temp = 0.0;
		for(size_type j=0;j<n;j++)
			temp += std::abs<real_value_type>(H(i,j)); // Correct me, if H is complex, can I use that abs?
		norm = std::max<real_value_type>(norm, temp);
	}
	if (norm == 0.0)
	{
		throw ::std::runtime_error("[expm_pad] Error: null input.");
	}
// Scaling, seek s such that || H*2^(-s) || < 1/2, and set scale = 2^(-s)
 	int s = 0;
	real_value_type scale = 1.0;
	if(norm > 0.5)
	{
		s = std::max<int>(0, static_cast<int>((log(norm) / log(2.0) + 2.0)));
		scale /= static_cast<real_value_type>(std::pow(2.0, s));
		U.assign(scale * H); // Here U is used as temp value due to that H is const
	}
// Horner evaluation of the irreducible fraction, see the following ref above.
// Initialise P (numerator) and Q (denominator)
	H2.assign( prod(U, U) );
	Q.assign( c(p+1)*I );
	P.assign( c(p)*I );
	size_type odd = 1;
	for( size_type k = p - 1; k > 0; --k)
	{
		if( odd == 1)
		{
			Q = ( prod(Q, H2) + c(k) * I );
		}
		else
		{
			P = ( prod(P, H2) + c(k) * I );
		}
		odd = 1 - odd;
	}
	if( odd == 1)
	{
		Q = ( prod(Q, U) );
		Q -= P ;
		//U.assign( -(I + 2*(Q\P)));
	}
	else
	{
		P = (prod(P, U));
		Q -= P;
		//U.assign( I + 2*(Q\P));
	}
// In origine expokit package, they use lapack ZGESV to obtain inverse matrix,
// and in that ZGESV routine, it uses LU decomposition for obtaing inverse matrix.
// Since in ublas, there is no matrix inversion template, I simply use the build-in
// LU decompostion package in ublas, and back substitute by myself.
//
//////////////// Implement Matrix Inversion ///////////////////////
	permutation_matrix<size_type> pm(n);
	int res = lu_factorize(Q, pm);
	if( res != 0)
	{
		throw ::std::runtime_error("[expm_pad] Error: matrix inversion in template expm_pad.");
	}
	H2 = I;  // H2 is not needed anymore, so it is temporary used as identity matrix for substituting.
	lu_substitute(Q, pm, H2);
	if( odd == 1)
		U.assign( -(I + 2.0 * prod(H2, P)));
 	else
		U.assign( I + 2.0 * prod(H2, P));
// Squaring
	for(size_t i = 0; i < s; ++i)
	{
		U = (prod(U,U));
	}
	return U;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_EXPM_HPP


#ifndef ALM_HERMITE_QUATERNION_CURVE_H
#define ALM_HERMITE_QUATERNION_CURVE_H

#include <Eigen/Dense>
#include <iostream>
#include <math.h>
#include <algorithm>

class HermiteQuaternionCurve{
public:
	HermiteQuaternionCurve(const Eigen::Quaterniond & quat_start, const Eigen::Vector3d & angular_velocity_start,
						   const Eigen::Quaterniond & quat_end, const Eigen::Vector3d & angular_velocity_end);
	~HermiteQuaternionCurve();

	// All values are expressed in "world frame"
	void evaluate(const double & s_in, Eigen::Quaterniond & quat_out);
	void getAngularVelocity(const double & s_in, Eigen::Vector3d & ang_vel_out);
	void getAngularAcceleration(const double & s_in, Eigen::Vector3d & ang_acc_out);

private:
	Eigen::Quaterniond qa; // Starting quaternion
	Eigen::Vector3d omega_a; // Starting Angular Velocity
	Eigen::Quaterniond qb; // Ending quaternion
	Eigen::Vector3d omega_b; // Ending Angular velocity

	Eigen::AngleAxisd omega_a_aa; // axis angle representation of omega_a
	Eigen::AngleAxisd omega_b_aa; // axis angle representation of omega_b

	void initialize_data_structures();

	void computeBasis(const double & s_in); // computes the basis functions
	void computeOmegas();

	Eigen::Quaterniond q0; // quat0
	Eigen::Quaterniond q1; // quat1
	Eigen::Quaterniond q2; // quat1
	Eigen::Quaterniond q3; // quat1

	double b1; // basis 1
	double b2; // basis 2
	double b3; // basis 3

	double bdot1; // 1st derivative of basis 1
	double bdot2; // 1st derivative of basis 2
	double bdot3; // 1st derivative of basis 3

	double bddot1; // 2nd derivative of basis 1
	double bddot2; // 2nd derivative of basis 2
	double bddot3; // 2nd derivative of basis 3

	Eigen::Vector3d omega_1;
	Eigen::Vector3d omega_2;
	Eigen::Vector3d omega_3;

	Eigen::AngleAxisd omega_1aa;
	Eigen::AngleAxisd omega_2aa;
	Eigen::AngleAxisd omega_3aa;

	// Allocate memory for quaternion operations
	Eigen::Quaterniond qtmp1;
	Eigen::Quaterniond qtmp2;
	Eigen::Quaterniond qtmp3;

	// progression variable
	double s_;
	// by default clamps within 0 and 1.
	double clamp(const double & s_in, double lo = 0.0, double hi = 1.0);

	void printQuat(const Eigen::Quaterniond & quat);

};

#endif


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/back.hpp>
#include <boost/hana/drop_front.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/range.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(hana::front(hana::range_c<int, 0, 5>) == hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::back(hana::range_c<unsigned long, 0, 5>) == hana::ulong_c<4>);
BOOST_HANA_CONSTANT_CHECK(hana::drop_front(hana::range_c<int, 0, 5>) == hana::make_range(hana::int_c<1>, hana::int_c<5>));

int main() { }


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra elementary factorial
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/algorithm/core/test/test_utils.h"
#include "fern/algorithm/algebra/elementary/factorial.h"

namespace fa = fern::algorithm;

template<
    class Value>
using OutOfDomainPolicy = fa::factorial::OutOfDomainPolicy<Value>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<int> policy;
        BOOST_CHECK( policy.within_domain(5));
        BOOST_CHECK(!policy.within_domain(-5));
        BOOST_CHECK( policy.within_domain(0));
        BOOST_CHECK( policy.within_domain(-0));
    }

    {
        OutOfDomainPolicy<double> policy;
        BOOST_CHECK( policy.within_domain(5));
        BOOST_CHECK(!policy.within_domain(-5));
        BOOST_CHECK(!policy.within_domain(-5.01));
        BOOST_CHECK(!policy.within_domain(-4.99));
        BOOST_CHECK( policy.within_domain(0));
        BOOST_CHECK( policy.within_domain(-0));
    }
}

template<
    class Value,
    class Result>
using OutOfRangePolicy = fa::factorial::OutOfRangePolicy<Value, Result>;

template<
    class Value,
    class Result>
struct VerifyWithinRange
{
    bool operator()(
        Value const& value)
    {
        fa::SequentialExecutionPolicy sequential;

        OutOfRangePolicy<Value, Result> policy;
        Result result;

        fa::algebra::factorial(sequential, value, result);

        return policy.within_range(value, result);
    }
};

BOOST_AUTO_TEST_CASE(out_of_range_policy)
{
    {
        VerifyWithinRange<uint8_t, uint8_t> verify;
        BOOST_CHECK_EQUAL(verify(0), true);
        BOOST_CHECK_EQUAL(verify(5), true);
        BOOST_CHECK_EQUAL(verify(6), false);
    }

    {
        VerifyWithinRange<int32_t, int32_t> verify;
        BOOST_CHECK_EQUAL(verify(0), true);
        BOOST_CHECK_EQUAL(verify(5), true);
        BOOST_CHECK_EQUAL(verify(6), true);
        BOOST_CHECK_EQUAL(verify(11), true);
        BOOST_CHECK_EQUAL(verify(13), false);
    }

    {
        VerifyWithinRange<uint32_t, uint32_t> verify;
        BOOST_CHECK_EQUAL(verify(0), true);
        BOOST_CHECK_EQUAL(verify(5), true);
        BOOST_CHECK_EQUAL(verify(11), true);
        BOOST_CHECK_EQUAL(verify(13), false);
    }

    {
        VerifyWithinRange<uint64_t, uint64_t> verify;
        BOOST_CHECK_EQUAL(verify(0), true);
        BOOST_CHECK_EQUAL(verify(5), true);
        BOOST_CHECK_EQUAL(verify(20), true);
        BOOST_CHECK_EQUAL(verify(21), false);
    }

    {
        VerifyWithinRange<float, float> verify;
        BOOST_CHECK_EQUAL(verify(34.0f), true);
        BOOST_CHECK_EQUAL(verify(35.0f), false);
        BOOST_CHECK_EQUAL(verify(fern::infinity<float>()), false);
    }

    {
        VerifyWithinRange<double, double> verify;
        BOOST_CHECK_EQUAL(verify(170.0), true);
        BOOST_CHECK_EQUAL(verify(171.0), false);
        BOOST_CHECK_EQUAL(verify(fern::infinity<double>()), false);
    }
}

template<
    class Value,
    class Result>
void verify_integral_value(
    Value const& value,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::factorial(sequential, value, result_we_get);

    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

template<
    class Value,
    class Result>
void verify_floating_point_value(
    Value const& value,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::factorial(sequential, value, result_we_get);

    BOOST_CHECK_CLOSE(result_we_get, result_we_want, 1e-6);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    {
        verify_integral_value<uint8_t, uint8_t>(0, 1);
        verify_integral_value<uint8_t, uint8_t>(1, 1);
        verify_integral_value<uint8_t, uint8_t>(2, 2);
        verify_integral_value<uint8_t, uint8_t>(3, 6);
        verify_integral_value<uint8_t, uint8_t>(5, 120);
    }

    {
        verify_integral_value<int32_t, int32_t>(0, 1);
        verify_integral_value<int32_t, int32_t>(1, 1);
        verify_integral_value<int32_t, int32_t>(2, 2);
        verify_integral_value<int32_t, int32_t>(3, 6);
        verify_integral_value<int32_t, int32_t>(6, 720);
        verify_integral_value<int32_t, int32_t>(12, 479001600);
    }

    {
        verify_floating_point_value<double, double>(0.0, 1.0);
        verify_floating_point_value<double, double>(1.0, 1.0);
        verify_floating_point_value<double, double>(10.0, 3628800.0);
    }
}


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_SYEV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_SYEV_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a real symmetric matrix A = Q * D * Q'
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * syev() computes the eigendecomposition of a N x N matrix
     * A = Q * D * Q',  where Q is a N x N orthogonal matrix and
     * D is a diagonal matrix. The diagonal elements D(i,i) is an
     * eigenvalue of A and Q(:,i) is a corresponding eigenvector.
     *
     * On return of syev, A is overwritten by Q and w contains the main
     * diagonal of D.
     *
     * int syev (char jobz, char uplo, A& a, W& w, minimal_workspace ) ;
     *    jobz : 'V' : compute eigenvectors
     *           'N' : do not compute eigenvectors
     *    uplo : 'U' : only the upper triangular part of A is used on input.
     *           'L' : only the lower triangular part of A is used on input.
     */

    namespace detail {

      inline
      void syev (char const jobz, char const uplo, integer_t const n,
                 float* a, integer_t const lda,
                 float* w, float* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_SSYEV (&jobz, &uplo, &n, a, &lda, w, work, &lwork, &info);
      }

      inline
      void syev (char const jobz, char const uplo, integer_t const n,
                 double* a, integer_t const lda,
                 double* w, double* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_DSYEV (&jobz, &uplo, &n, a, &lda, w, work, &lwork, &info);
      }

      template <typename A, typename W, typename Work>
      int syev (char jobz, char uplo, A& a, W& w, Work& work) {

/*#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<A>::matrix_structure,
          traits::general_t
        >::value));
#endif*/

        integer_t const n = traits::matrix_size1 (a);
        assert ( n>0 );
        assert (traits::matrix_size2 (a)==n);
        assert (traits::leading_dimension (a)>=n);
        assert (traits::vector_size (w)==n);
        assert (3*n-1 <= traits::vector_size (work));
        assert ( uplo=='U' || uplo=='L' );
        assert ( jobz=='N' || jobz=='V' );

        integer_t info;
        detail::syev (jobz, uplo, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (w),
                     traits::vector_storage (work),
                     traits::vector_size (work),
                     info);
        return info;
      }
    }  // namespace detail

    // Function that allocates work arrays
    template <typename A, typename W>
    int syev (char jobz, char uplo, A& a, W& w, optimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,34*n) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W>
    int syev (char jobz, char uplo, A& a, W& w, minimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,3*n-1) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W, typename Work>
    int syev (char jobz, char uplo, A& a, W& w, detail::workspace1<Work> workspace ) {
       typedef typename traits::matrix_traits<A>::value_type value_type ;

       return detail::syev(jobz, uplo, a, w, workspace.select(value_type()));
    } // syev()

    // Function without workarray as argument
    template <typename A, typename W>
    inline
    int syev (char jobz, char uplo, A& a, W& w) {
       return syev(jobz, uplo, a, w, optimal_workspace());
    } // syev()

    //
    // With UPLO integrated in matrix type (this is not possible
    // since a contains the eigenvectors afterwards and thus A cannot be symmetric)
    //
    template <typename A, typename W>
    int syev (char jobz, A& a, W& w, optimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);
       char uplo = traits::matrix_uplo_tag( a ) ;
/*#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif*/

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,34*n) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W>
    int syev (char jobz, A& a, W& w, minimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);
       char uplo = traits::matrix_uplo_tag( a ) ;
/*#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif*/
       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,3*n-1) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W, typename Work>
    int syev (char jobz, A& a, W& w, detail::workspace1<Work> workspace ) {
       typedef typename A::value_type value_type ;
       char uplo = traits::matrix_uplo_tag( a ) ;
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif
       return detail::syev(jobz, uplo, a, w, workspace.select(value_type()));
    } // syev()

    // Function without workarray as argument
    template <typename A, typename W>
    inline
    int syev (char jobz, A& a, W& w) {
       char uplo = traits::matrix_uplo_tag( a ) ;
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif
       return syev(jobz, uplo, a, w, optimal_workspace());
    } // syev()

  }

}}}

#endif


/** @file layouts.cc
 * @author David F. Gleich
 * @date 2008-09-25
 * @copyright Stanford University, 2008
 * Graph layout wrappers
 */

/** History
 *  2008-09-25: Initial coding
 *  2008-09-27: Fixed progressive calls
 */

#include "include/matlab_bgl.h"

#include <yasmic/undir_simple_csr_matrix_as_graph.hpp>
#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <boost/graph/kamada_kawai_spring_layout.hpp>
#include <yasmic/boost_mod/fruchterman_reingold.hpp>
#include <boost/graph/gursoy_atun_layout.hpp>
#include <boost/graph/circle_layout.hpp>
#include <boost/graph/random_layout.hpp>
#include <boost/graph/simple_point.hpp>

// for constant_value_property_map
#include <yasmic/boost_mod/core_numbers.hpp>

#include <vector>
#include <iostream>
#include <algorithm>

#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include "libmbgl_util.hpp"

/** Monitor tolerance for Kamada-Kawai layout with a maximum iteration limit
 * This class fixes problems with the boost layout_tolerance code.
 */
template <typename T = double>
class layout_and_iteration_tolerance
{
public:
  layout_and_iteration_tolerance(const T& tolerance=0.001, int maxiter = 100)
  : maxiter(maxiter), iter(0), tolerance(tolerance),
    first_energy((std::numeric_limits<T>::max)()),
    last_energy((std::numeric_limits<T>::max)()),
    first_local_energy((std::numeric_limits<T>::max)()),
    last_local_energy((std::numeric_limits<T>::max)()) { }

  template<typename Graph>
  bool
  operator()(T delta_p,
              typename boost::graph_traits<Graph>::vertex_descriptor p,
              const Graph& g,
              bool global)
  {
    bool done = false;
    if (global) {
      if (first_energy == (std::numeric_limits<T>::max)()) {
        first_energy = delta_p;
        last_energy = delta_p;
        return delta_p < (std::numeric_limits<T>::epsilon)();
      }
      T diff = last_energy - delta_p;
      if (diff < T(0)) diff = -diff;
      done = (delta_p < (std::numeric_limits<T>::epsilon)()
              || diff/first_energy < tolerance);
      last_energy = delta_p;
    } else {
      if (first_local_energy == (std::numeric_limits<T>::max)()) {
        first_local_energy = delta_p;
        last_local_energy = delta_p;
        return delta_p < (std::numeric_limits<T>::epsilon)();
      }
      T diff = last_local_energy - delta_p;
      // uncommenting the following line causes the layout to cycle
      // if (diff < T(0)) diff = -diff;
      done = (delta_p < (std::numeric_limits<T>::epsilon)()
              || diff/first_local_energy < tolerance);
      last_local_energy = delta_p;
    }
    if (!done && global) {
      iter++;
      done = iter>maxiter;
    }
    return done;
  }

private:
  int maxiter, iter;
  T tolerance;
  T first_energy;
  T last_energy;
  T first_local_energy;
  T last_local_energy;
};

/** Compute a spring layout of a graph
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param weight the weight of each edge (can be NULL for unweighted graphs)
 * @param tol the stopping tolerance in terms of layout change
 * @param iterations the maximum number of global layout iterations
 * @param spring_constant the spring constant
 * @param progressive a binary value (0 or 1) if we should reuse the positions
 * @param positions an array of positions, length nverts*2
 * @param spring_strength a matrix of spring strengths between vertices,
 *           size nverts-by-nverts
 * @param distance a matrix of distances between vertices,
 *           size nverts-by-nverts
 */
int kamada_kawai_spring_layout(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight,
    double tol, int iterations, double spring_constant, int progressive,
    double edge_length,
    double *positions,
    double *spring_strength, double *distance)
{
  using namespace yasmic;
  using namespace boost;
  typedef undir_simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);
  assert(nverts == 0 || positions);
  if (tol < 0 || spring_strength == NULL || distance == NULL) { return -1; }
  std::vector<simple_point<double> > position_vec(nverts);
  std::vector<std::pair<double,double> > partial_deriv_vec(nverts);
  if (!progressive) {
    // initial random layout
    circle_graph_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        (double)nverts*edge_length/(2*M_PI));
  } else {
    // copy the layout from positions
    mbglIndex n = num_vertices(g);
    for (mbglIndex i = 0; i<n; i++) {
      position_vec[i].x = positions[i+0*n];
      position_vec[i].y = positions[i+1*n];
    }
  }
  bool rval = false;
  if (weight == NULL) {
    rval = kamada_kawai_spring_layout(g,
      make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
      boost::detail::constant_value_property_map<double>(1.0), // edge_weight
      boost::edge_length(edge_length), // edge_or_side_length
      layout_and_iteration_tolerance<double>(tol,iterations), // done
      spring_constant,
      get(vertex_index,g),
      row_matrix<double>(distance,nverts,nverts),
      row_matrix<double>(spring_strength,nverts,nverts),
      make_iterator_property_map(partial_deriv_vec.begin(),get(vertex_index,g)));
  } else {
    rval = kamada_kawai_spring_layout(g,
      make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
      get(edge_weight,g), // edge_weight
      boost::edge_length(edge_length), // edge_or_side_length
      layout_and_iteration_tolerance<double>(tol,iterations), // done
      spring_constant,
      get(vertex_index,g),
      row_matrix<double>(distance,nverts,nverts),
      row_matrix<double>(spring_strength,nverts,nverts),
      make_iterator_property_map(partial_deriv_vec.begin(),get(vertex_index,g)));
  }
  if (rval) {
    mbglIndex n = num_vertices(g);
    for (mbglIndex i = 0; i<n; i++) {
      positions[i+0*n] = position_vec[i].x;
      positions[i+1*n] = position_vec[i].y;
    }
    return 0;
  } else {
    return -2;
  }
}

/** Compute a force directed layout of a graph
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param iterations the number of iterations to run
 * @param initial_temp the initial temperature of the system
 * @param grid_force_pairs a binary value (0 or 1) if the alg should use
 *   a grid to compute the force between pairs
 * @param width the total width of the layout
 * @param height the total height of the layout
 * @param progressive a binary value (0 or 1) if we should start from
 *   the positions in the positions value
 * @param positions an array of positions, length nverts*2
 */
int fruchterman_reingold_force_directed_layout(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int iterations, double initial_temp, int grid_force_pairs,
    double width, double height, int progressive,
    double *positions)
{
  using namespace yasmic;
  using namespace boost;
  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
  assert(nverts == 0 || positions);
  std::vector<simple_point<double> > position_vec(nverts);
  if (width <= 0 || height <= 0 || iterations <= 0) { return -1; }
  if (!progressive) {
    // initial random layout
    minstd_rand gen;
    random_graph_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        -width/2.0, width/2.0, -height/2.0, height/2.0, gen);
  } else {
    // copy the layout from positions
    mbglIndex n = num_vertices(g);
    for (mbglIndex i = 0; i<n; i++) {
      position_vec[i].x = positions[i+0*n];
      position_vec[i].y = positions[i+1*n];
    }
  }
  if (grid_force_pairs) {
    fruchterman_reingold_force_directed_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        width, height,
        cooling(linear_cooling<double>(iterations, initial_temp)));
  } else {
    fruchterman_reingold_force_directed_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        width, height,
        cooling(linear_cooling<double>(iterations, initial_temp)).
          force_pairs(all_force_pairs()));
  }
  // copy the positions over
  mbglIndex n = num_vertices(g);
  for (mbglIndex i = 0; i<n; i++) {
    positions[i+0*n] = position_vec[i].x;
    positions[i+1*n] = position_vec[i].y;
  }
  return 0;
}

/** A helper function to allocate the points and convert back to positions
 */
template <typename Graph, typename Topology>
int gursoy_atun_layout_helper(const Graph& g, bool weighted,
    Topology space, int space_dim, int nsteps,
    double diameter_i, double diameter_f, double lc_i, double lc_f,
    double *positions)
{
  using namespace boost;
  std::vector<typename Topology::point_type> position_map(num_vertices(g));
  if (weighted) {
    gursoy_atun_layout(g, space,
      make_iterator_property_map(position_map.begin(),get(vertex_index,g)),
      nsteps, diameter_i, diameter_f, lc_i, lc_f, get(vertex_index,g),
      get(edge_weight,g));
  } else {
    gursoy_atun_layout(g, space,
          make_iterator_property_map(position_map.begin(),get(vertex_index,g)),
          nsteps, diameter_i, diameter_f, lc_i, lc_f, get(vertex_index,g));
  }
  // copy the positions over
  mbglIndex n = num_vertices(g);
  mbglIndex numdim = (mbglIndex)space_dim;
  for (mbglIndex i = 0; i<n; i++) {
    for (mbglIndex d = 0; d<numdim; d++) {
      positions[i+d*n] = position_map[i][d];
    }
  }
  return 0;
}

const int gursoy_atun_invalid_dim = -10;
const int gursoy_atun_dim_too_large = -11;
const int gursoy_atun_max_dim = 10;

/** Compute a topologically uniform layout
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param weight the weight of each edge (can be NULL for unweighted graphs)
 * @param topology the topology type
 * @param topology_dim the topology dimension
 * @param iterations the number of iterations
 * @param diameter_i the initial diameter for modifications
 * @param diameter_f the final diameter for modifications
 * @param learning_constant_i the initial learning constant
 * @param learning_constant_f the final learning constant
 * @param positions an array of positions, length nverts*topology_dim
 */
int gursoy_atun_layout(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight,
    gursoy_atun_layout_topology_t topology, int topology_dim,
    int iterations, double diameter_i, double diameter_f,
    double learning_constant_i, double learning_constant_f,
    double *positions)
{
  using namespace yasmic;
  using namespace boost;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);
  assert(nverts == 0 || positions);

  switch (topology) {
  case BALL_LAYOUT_TOPOLOGY:
    if (topology_dim<2) { return gursoy_atun_invalid_dim; }
    if (topology_dim>gursoy_atun_max_dim) { return gursoy_atun_dim_too_large; }
    switch (topology_dim) {
    case 2:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<2>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 3:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<3>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 4:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<4>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 5:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<5>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 6:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<6>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 7:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<7>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 8:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<8>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 9:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<9>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 10:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<10>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    }
    break;
  case CUBE_LAYOUT_TOPOLOGY:
    if (topology_dim<2) { return gursoy_atun_invalid_dim; }
    if (topology_dim>gursoy_atun_max_dim) { return gursoy_atun_dim_too_large; }
    switch (topology_dim) {
    case 2:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<2>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 3:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<3>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 4:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<4>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 5:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<5>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 6:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<6>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 7:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<7>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 8:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<8>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 9:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<9>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 10:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<10>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    }
    break;
  case HEART_LAYOUT_TOPOLOGY:
    if (topology_dim!=2) { return gursoy_atun_invalid_dim; }
    gursoy_atun_layout_helper(g, weight!=NULL,
      heart_topology<>(),topology_dim,iterations,diameter_i,diameter_f,
      learning_constant_i, learning_constant_f, positions);
    break;
  default:
    return -1;
  }
  return 0;
}


//                Copyright Robert J McCabe 2015.
// 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)
//
//     Please report any bugs, typos, or suggestions to
//         https://github.com/rjmccabe3701/stat_log/issues

#include "common.h"
#include "hw_intf_stat_tags.h"
#include <stat_log/stat_log.h>
#include <stat_log/backends/shared_mem_backend.h>
#include <stat_log/loggers/shared_memory_logger.h>
#include <stat_log/util/compile_proxy.h>

//Statistics
#include <stat_log/stats/stats_common.h>
#include <stat_log/stats/simple_counter.h>
#include <stat_log/stats/simple_status.h>
#include <stat_log/stats/stat_array.h>
#if 1
#include <stat_log/stats/accumulator.h>
#include <stat_log/stats/accumulator_types/count.h>
#include <stat_log/stats/accumulator_types/min.h>
#include <stat_log/stats/accumulator_types/max.h>
#include <stat_log/stats/accumulator_types/mean.h>
#include <stat_log/stats/accumulator_types/histogram.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#endif

#include <type_traits>

#define STAT_LOG_MAC_SIS_SHM_NAME "STAT_LOG_MAC_SIS"
#define STAT_LOG_HW_INTF_SHM_NAME "STAT_LOG_HW_INTF"
#define STAT_LOG_TEXT_LOGGER_NAME "SHM_LOGGER_TEXT"
#define STAT_LOG_TEXT_LOGGER_SIZE_BYTES 4194304
#define STAT_LOG_HEXDUMP_LOGGER_NAME "SHM_LOGGER_HEXDUMP"
#define STAT_LOG_HEXDUMP_LOGGER_SIZE_BYTES 4194304

using MacSisOpStat = stat_log::LogStatOperational<MAC_SIS_STATS, MAC_SIS_LOG>;
using MacSisControlStat = stat_log::LogStatControl<MAC_SIS_STATS, MAC_SIS_LOG>;

using HwIntfOpStat = stat_log::LogStatOperational<hw_intf::HW_INTF_STATS, hw_intf::HW_INTF_LOG>;
using HwIntfControlStat = stat_log::LogStatControl<hw_intf::HW_INTF_STATS, hw_intf::HW_INTF_LOG>;

using LogGen = stat_log::shared_mem_logger_generator;
using LogRet = stat_log::shared_mem_logger_retriever;

/*********************************
 * Statistic definitions
 *********************************/
namespace stat_log
{
   namespace ba = boost::accumulators;
   //Default stat
   template <typename Tag, class Enable>
   struct stat_tag_to_type
   {
      using type = SimpleCounter<int>;
   };

   //Mac: IP sent: Histogram of IP pkt sizes sizes
   template <>
   struct stat_tag_to_type<mac::IP_PKT_SENT_SIZE_TAG>
   {
      using type = Accumulator<
         stat_log::HistogramCount<
            int,
            300, //start bin
            1500, //stop bin
            10 //num_bits
         >
      >;
   };

   //Mac: IP rcvd: summary statistics of IP pkt sizes:
   //  count, min, max, mean
   template <>
   struct stat_tag_to_type<mac::IP_PKT_RCVD_SIZE_TAG>
   {
      using type = Accumulator<
         ba::accumulator_set<
               double
               , ba::stats<
                  ba::tag::count
                  , ba::tag::min
                  , ba::tag::max
                  , ba::tag::mean
               >
            >
         >;
   };

   //Mac: tx power level: array of power levels (ints)
   template <>
   struct stat_tag_to_type<mac::TX_POWER_LEVEL_TAG>
   {
      using type = StatArray<sis::max_nbrs, SimpleStatus<int>>;
   };

   //Mac: rx power level: array of received powers in dB (doubles)
   template <>
   struct stat_tag_to_type<mac::RX_POWER_LEVEL_TAG>
   {
      using type = StatArray<sis::max_nbrs, SimpleStatus<double>>;
   };

   //Sis: prop delay: per-nbr density histogram of delays
   template <>
   struct stat_tag_to_type<sis::PROP_DELAY_TAG>
   {
      using type = StatArray<sis::max_nbrs,
               Accumulator<
                  stat_log::HistogramDensity<
                  // stat_log::HistogramCount<
                     double,
                     50, //start bin
                     400, //stop bin
                     7 //num_bits
                  >
               >
            >;
   };

   //Sis: channel quality: per-nbr, per-frequency channel quality enumeration
   template <>
   struct stat_tag_to_type<sis::CHANNEL_QUALITY_TAG>
   {
      using type = StatArray<sis::max_nbrs,
                                StatArray<sis::num_frequencies,
                                    SimpleStatus<int>
                                 >
                            >;
   };

   //Sis: frame rx status: array of counters (per nbr and rx status)
   template <>
   struct stat_tag_to_type<sis::FRAME_RX_STATUS_TAG>
   {
      using type = StatArray<sis::max_nbrs,
                                StatArray<sis::num_rx_frame_status,
                                    SimpleCounter<int>
                                 >
                            >;
   };

   //Sis: link status: per-nbr link status indicator (int)
   template <>
   struct stat_tag_to_type<sis::LINK_STATUS_TAG>
   {
      using type = StatArray<sis::max_nbrs, SimpleStatus<int>>;
   };

   //HwIntf: fault: array of counters (per each fault type)
   template <>
   struct stat_tag_to_type<hw_intf::FPGA_FAULT_TAG>
   {
      using type = StatArray<hw_intf::num_fpga_faults, int>;
   };

   //HwIntf: interrupt: array of counters (per each interrupt)
   template <>
   struct stat_tag_to_type<hw_intf::INTERRUPT_TAG>
   {
      using type = StatArray<hw_intf::num_fpga_interrupts, int>;
   };
}

/*********************************
 * common logger and initializer definitions
 *********************************/
namespace
{
   using namespace stat_log;
   template <typename Tag>
   inline LogGenProxy logCommon(LogLevel ll, int log_idx)
   {
      constexpr auto is_mac_sis_tag = TagBelongsToLog<Tag, MacSisOpStat>::value;
      constexpr auto is_hw_intf_tag = TagBelongsToLog<Tag, HwIntfOpStat>::value;
      static_assert(is_mac_sis_tag || is_hw_intf_tag, "Bad tag in logCommon!\n");
      using TheOpStat = std::conditional_t<
         is_mac_sis_tag, MacSisOpStat, HwIntfOpStat>;
      TheOpStat& stat = stat_log::getStatLogSingleton<TheOpStat>();
      return stat.template getLog<Tag>(ll, log_idx);
   }

   template <typename MacSisStatType, typename HwIntfStatType, typename LogType>
   void initializeCommon()
   {
      auto& macSisStat = stat_log::getStatLogSingleton<MacSisStatType>();
      auto& hwIntfStat = stat_log::getStatLogSingleton<HwIntfStatType>();

      macSisStat.init(STAT_LOG_MAC_SIS_SHM_NAME);
      hwIntfStat.init(STAT_LOG_HW_INTF_SHM_NAME);
      auto text_logger = std::make_shared<LogType>(
            STAT_LOG_TEXT_LOGGER_NAME,
            STAT_LOG_TEXT_LOGGER_SIZE_BYTES);

      macSisStat.addLogger(text_logger);
      hwIntfStat.addLogger(text_logger);

      auto hexdump_logger = std::make_shared<LogType>(
            STAT_LOG_HEXDUMP_LOGGER_NAME,
            STAT_LOG_HEXDUMP_LOGGER_SIZE_BYTES);

      macSisStat.addLogger(hexdump_logger);
      hwIntfStat.addLogger(hexdump_logger);
   }
}

/*********************************
 * definitions of the "compile_proxy" API's methods
 *********************************/
namespace stat_log
{
   template <typename Tag>
   LogGenProxy logger(LogLevel ll)
   {
      return logCommon<Tag>(ll, 0);
   }

   template <typename Tag>
   LogGenProxy hexDumper(LogLevel ll)
   {
      return logCommon<Tag>(ll, 1);
   }

   template <typename Tag, typename ...Args>
   void writeStat(Args... args)
   {
      constexpr auto is_mac_sis_tag = TagBelongsToStat<Tag, MacSisOpStat>::value;
      constexpr auto is_hw_intf_tag = TagBelongsToStat<Tag, HwIntfOpStat>::value;
      static_assert(is_mac_sis_tag || is_hw_intf_tag, "Bad tag in writeStat!\n");
      using TheOpStat = std::conditional_t<
         is_mac_sis_tag, MacSisOpStat, HwIntfOpStat>;
      stat_log::getStatLogSingleton<TheOpStat>().template writeStat<Tag>(args...);
   }

   //EXPLICIT TEMPLATE INSTANTIATIONS
   //MAC
   template void writeStat<mac::IP_PKTS_SENT_TAG>(int val);
   template void writeStat<mac::IP_PKTS_RCVD_TAG>(int val);
   template void writeStat<mac::IP_PKT_SENT_SIZE_TAG>(int val);
   template void writeStat<mac::IP_PKT_RCVD_SIZE_TAG>(int val);
   template void writeStat<mac::TX_POWER_LEVEL_TAG>(int idx, int val);
   template void writeStat<mac::RX_POWER_LEVEL_TAG>(int idx, double val);

   //SIS
   template void writeStat<sis::PROP_DELAY_TAG>(int nbr_idx, double delay_us);
   template void writeStat<sis::CHANNEL_QUALITY_TAG>(int nbr_idx, int freq_idx, double qual);
   template void writeStat<sis::FRAME_RX_STATUS_TAG>(int nbr_idx, int rx_status_enum, int val);
   template void writeStat<sis::LINK_STATUS_TAG>(int nbr_idx, int val);

   //HW_INTF
   template void writeStat<hw_intf::FPGA_FAULT_TAG>(int fault_idx, int val);
   template void writeStat<hw_intf::BYTES_SENT_TAG>(int val);
   template void writeStat<hw_intf::BYTES_RCVD_TAG>(int val);
   template void writeStat<hw_intf::INTERRUPT_TAG>(int interrupt_idx, int val);

   template LogGenProxy logger<hw_intf::HW_INTF_LOG>(LogLevel ll);
   template LogGenProxy hexDumper<hw_intf::HW_INTF_LOG>(LogLevel ll);
   template LogGenProxy logger<mac::MAC_LOG>(LogLevel ll);
   template LogGenProxy hexDumper<mac::MAC_LOG>(LogLevel ll);
   template LogGenProxy logger<sis::SIS_LOG>(LogLevel ll);
   template LogGenProxy hexDumper<sis::SIS_LOG>(LogLevel ll);

//TODO: this will be super annoying for the user to have to define the
// stat hierarchy AND explicitly instantiate each of them ...
// Think  of a way to automate this.

}

template <>
void initializeStatistics<false == IsOperational>()
{
    initializeCommon<MacSisControlStat, HwIntfControlStat, LogRet>();
}

template <>
void initializeStatistics<true == IsOperational>()
{
   initializeCommon<MacSisOpStat, HwIntfOpStat, LogGen>();
}

void handleCommandLineArgs(int argc, char** argv)
{
   auto& macSisControlStat = getStatLogSingleton<MacSisControlStat>();
   auto& hwIntfControlStat = getStatLogSingleton<HwIntfControlStat>();

   //Assign a few enumeration and dimension names to make stat-viewing pretty.
   // TODO: design this better
#if 0
   hwIntfControlStat.assignEnumerationNames<
      hw_intf::MISC_FPGA_FAULT_TAG>({"OVER_TEMP", "INVALID_COMMAND", "UNKNOWN_FAULT"});

   macSisControlStat.assignEnumerationNames<
      sis::MAC_PKTS_DOWN_TAG>({"L", "M", "H"});

   macSisControlStat.assignDimensionNames<
      sis::MAC_PKTS_DOWN_TAG>({"Priority", "Traffic Type"});
#endif

   macSisControlStat.parseUserCommands(argc, argv);
   hwIntfControlStat.parseUserCommands(argc, argv);
   macSisControlStat.showOutput();
}


#ifndef TEST_UNIT_TORSTEN_COUPLED_ONECPT_MODEL_TEST_FIXTURE
#define TEST_UNIT_TORSTEN_COUPLED_ONECPT_MODEL_TEST_FIXTURE

#include <stan/math/rev/core/recover_memory.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <stan/math/torsten/test/unit/test_fixture_model.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/pmx_solve_onecpt_rk45.hpp>
#include <stan/math/torsten/pmx_solve_onecpt_bdf.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

struct CoupledOneCptODE {
  /**
   * Coupled model functor
   */
  template <typename T0, typename T1, typename T2, typename T3>
  inline
  Eigen::Matrix<typename stan::return_type_t<T0, T1, T2, T3>, -1, 1>
  operator()(const T0& t,
             const Eigen::Matrix<T1, -1, 1>& x,
             const Eigen::Matrix<T2, -1, 1>& x_pk,
             const std::vector<T3>& theta,
             const std::vector<double>& x_r,
             const std::vector<int>& x_i,
             std::ostream* pstream_) const {
    typedef typename boost::math::tools::promote_args<T0, T1, T2, T3>::type
      scalar;

    scalar
      VC = theta[1],
      Mtt = theta[3],
      circ0 = theta[4],
      alpha = theta[5],
      gamma = theta[6],
      ktr = 4 / Mtt,
      prol = x[0] + circ0,
      transit = x[1] + circ0,
      circ = x[2] + circ0,
      conc = x_pk[1] / VC,
      Edrug = alpha * conc;

    Eigen::Matrix<scalar, -1, 1> dxdt(3);
    dxdt << ktr * prol * ((1 - Edrug) * pow(circ0 / circ, gamma) - 1),
      ktr * (prol - transit),
      ktr * (transit - circ);

    return dxdt;
  }

  /**
   * full ODE model functor
   */
  template <typename T0, typename T1, typename T2>
  inline
  Eigen::Matrix<typename stan::return_type_t<T0, T1, T2>, -1, 1>
  operator()(const T0& t,
             const Eigen::Matrix<T1, -1, 1>& x,
             std::ostream* pstream_,
             const std::vector<T2>& theta,
             const std::vector<double>& x_r,
             const std::vector<int>& x_i) const {
    using scalar = typename stan::return_type_t<T0, T1, T2>;

    T2 CL = theta[0];
    T2 VC = theta[1];
    T2 ka = theta[2];
    T2 Mtt = theta[3];
    T2 circ0 = theta[4];
    T2 alpha = theta[5];
    T2 gamma = theta[6];
    T2 ktr = 4.0 / Mtt;
    scalar prol = x[2] + circ0;
    scalar transit = x[3] + circ0;
    scalar circ = x[4] + circ0;
    scalar Edrug = alpha * x[1] / VC;

    Eigen::Matrix<scalar, -1, 1>  dxdt(5);
    dxdt << - ka * x[0],
      ka * x[0] - CL / VC * x[1],
      ktr * prol * ((1 - Edrug) * pow(circ0 / circ, gamma) - 1),
      ktr * (prol - transit),
      ktr * (transit - circ);

    return dxdt;
  }
};

template<typename T>
struct test_coupled_onecpt : public TorstenPMXTest<test_coupled_onecpt<T> > {
  const size_t nOde;
  test_coupled_onecpt() : nOde(5) {
    this -> ncmt = nOde;           // # of PD compartments
    this -> reset_events(10);
    this -> theta.resize(1);
    this -> theta[0] = {10, 35, 2.0, 125, 5, 3e-4, 0.17};
    this -> biovar.resize(1);
    this -> biovar[0] = {1, 1, 1, 1, 1};
    this -> tlag.resize(1);
    this -> tlag[0] = {0, 0, 0, 0, 0};

    for(int i = 0; i < this -> nt; i++) {
      this -> time[i] = i * 0.25;
    }
    this -> time[this -> nt - 1] = 4.0;

    this -> amt[0] = 10000.0;
    this -> cmt[0] = 1;
    this -> evid[0] = 1;
  }
};

#endif


#ifndef STAN_MATH_PRIM_MAT_PROB_DIRICHLET_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_DIRICHLET_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/mat/fun/log_sum_exp.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return a draw from a Dirichlet distribution with specified
 * parameters and pseudo-random number generator.
 *
 * For prior counts greater than zero, the usual algorithm that
 * draws gamma variates and normalizes is used.
 *
 * For prior counts less than zero (i.e., parameters with value
 * less than one), a log-scale version of the following algorithm
 * is used to deal with underflow:
 *
 * <blockquote>
 * G. Marsaglia and W. Tsang. A simple method for generating gamma
 * variables. ACM Transactions on Mathematical Software.
 * 26(3):363--372, 2000.
 * </blockquote>
 *
 * @tparam RNG Type of pseudo-random number generator.
 * @param alpha Prior count (plus 1) parameter for Dirichlet.
 * @param rng Pseudo-random number generator.
 */
template <class RNG>
inline Eigen::VectorXd dirichlet_rng(
    const Eigen::Matrix<double, Eigen::Dynamic, 1>& alpha, RNG& rng) {
  using Eigen::VectorXd;
  using boost::gamma_distribution;
  using boost::random::uniform_real_distribution;
  using boost::variate_generator;
  using std::exp;
  using std::log;

  // separate algorithm if any parameter is less than 1
  if (alpha.minCoeff() < 1) {
    variate_generator<RNG&, uniform_real_distribution<> > uniform_rng(
        rng, uniform_real_distribution<>(0.0, 1.0));
    VectorXd log_y(alpha.size());
    for (int i = 0; i < alpha.size(); ++i) {
      variate_generator<RNG&, gamma_distribution<> > gamma_rng(
          rng, gamma_distribution<>(alpha(i) + 1, 1));
      double log_u = log(uniform_rng());
      log_y(i) = log(gamma_rng()) + log_u / alpha(i);
    }
    double log_sum_y = log_sum_exp(log_y);
    VectorXd theta(alpha.size());
    for (int i = 0; i < alpha.size(); ++i) {
      theta(i) = exp(log_y(i) - log_sum_y);
    }
    return theta;
  }

  // standard normalized gamma algorithm
  Eigen::VectorXd y(alpha.rows());
  for (int i = 0; i < alpha.rows(); i++) {
    variate_generator<RNG&, gamma_distribution<> > gamma_rng(
        rng, gamma_distribution<>(alpha(i, 0), 1e-7));
    y(i) = gamma_rng();
  }
  return y / y.sum();
}

}  // namespace math
}  // namespace stan
#endif


#if defined(WITH_CPLEX) || defined(WITH_GUROBI)

#include <boost/python.hpp>
#include <boost/python/module.hpp>
#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>

#include <opengm/inference/icm.hxx>
#include <opengm/learning/maximum_likelihood_learning.hxx>

#define DefaultErrorFn DefaultErrorFn_TrwsExternal_ML
#include "helper.hxx"

namespace bp = boost::python;
namespace op = opengm::python;
namespace ol = opengm::learning;

namespace opengm{

    template<class PARAM>
    PARAM * pyMaxLikelihoodParamConstructor(
	size_t maximumNumberOfIterations=100,
	double gradientStepSize=0.1,
	double weightStoppingCriteria=0.00000001,
	double gradientStoppingCriteria=0.00000001,
	bool infoFlag=true,
	bool infoEveryStep=false,
	double weightRegularizer = 1.0,
	size_t beliefPropagationMaximumNumberOfIterations = 20,
	double beliefPropagationConvergenceBound = 0.0001,
	double beliefPropagationDamping = 0.5,
	double beliefPropagationTemperature = 0.3,
	opengm::Tribool beliefPropagationIsAcyclic=opengm::Tribool(opengm::Tribool::Maybe)
    ){
        PARAM * p  = new PARAM();
	p->maximumNumberOfIterations_ = maximumNumberOfIterations;
	p->gradientStepSize_ = gradientStepSize;
	p->weightStoppingCriteria_ = weightStoppingCriteria;
	p->gradientStoppingCriteria_ = gradientStoppingCriteria;
	p->infoFlag_ = infoFlag;
	p->infoEveryStep_ = infoEveryStep;
	p->weightRegularizer_ = weightRegularizer;
	p->beliefPropagationMaximumNumberOfIterations_ = beliefPropagationMaximumNumberOfIterations;
	p->beliefPropagationConvergenceBound_ = beliefPropagationConvergenceBound;
	p->beliefPropagationDamping_ = beliefPropagationDamping;
	p->beliefPropagationTemperature_ = beliefPropagationTemperature;
	p->beliefPropagationIsAcyclic_ = beliefPropagationIsAcyclic;
        return p;
    }

    template<class DATASET>
    void export_max_likelihood_learner(const std::string & clsName){
        typedef learning::MaximumLikelihoodLearner<DATASET> PyLearner;
        typedef typename PyLearner::Parameter PyLearnerParam;
        typedef typename PyLearner::DatasetType DatasetType;

        const std::string paramClsName = clsName + std::string("Parameter");

        bp::class_<PyLearnerParam>(paramClsName.c_str(), bp::init<>())
	  .def("__init__", make_constructor(&pyMaxLikelihoodParamConstructor<PyLearnerParam> ,boost::python::default_call_policies()))
	  //.def_readwrite("maxIterations", &PyLearnerParam::maximumNumberOfIterations_)
        ;

        boost::python::class_<PyLearner>( clsName.c_str(), boost::python::init<DatasetType &, const PyLearnerParam &>() )
            .def("learn",&PyLearner::learn)
        ;
    }

  //template void
  //export_max_likelihood_learner<op::GmAdderHammingLossDataset> (const std::string& className);

    template void
    export_max_likelihood_learner<op::GmAdderFlexibleLossDataset> (const std::string& className);
}

#endif


// Copyright (C) 2019 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

// This file was created by Steffen Urban (urbste@googlemail.com) October 2019

#include "theia/sfm/estimators/estimate_radial_dist_uncalibrated_absolute_pose.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <vector>

#include "theia/sfm/camera/projection_matrix_utils.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/four_point_focal_length_radial_distortion.h"
#include "theia/sfm/types.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

void DistortPoint(const Eigen::Vector2d& point2d, const double& distortion,
                  Eigen::Vector2d* distorted_point) {
  const double r_u_sq = point2d[0] * point2d[0] + point2d[1] * point2d[1];

  const double denom = 2.0 * distortion * r_u_sq;
  const double inner_sqrt = 1.0 - 4.0 * distortion * r_u_sq;

  // If the denominator is nearly zero then we can evaluate the distorted
  // coordinates as k or r_u^2 goes to zero. Both evaluate to the identity.
  if (std::abs(denom) < 1e-15 || inner_sqrt < 0.0) {
    (*distorted_point)[0] = point2d[0];
    (*distorted_point)[1] = point2d[1];
  } else {
    const double scale = (1.0 - std::sqrt(inner_sqrt)) / denom;
    (*distorted_point)[0] = point2d[0] * scale;
    (*distorted_point)[1] = point2d[1] * scale;
  }
}

using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
// An estimator for computing the uncalibrated absolute pose from 4 feature
// correspondences. The feature correspondences should be normalized such that
// the principal point is at (0, 0).
class RadialDistUncalibratedAbsolutePoseEstimator
    : public Estimator<FeatureCorrespondence2D3D,
                       RadialDistUncalibratedAbsolutePose> {
 public:
  RadialDistUncalibratedAbsolutePoseEstimator() {}

  // 4 correspondences are needed to determine the absolute pose.
  double SampleSize() const { return 4; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence2D3D>& correspondences,
      std::vector<RadialDistUncalibratedAbsolutePose>* absolute_poses) const {
    const Vector2d features[4] = {
        correspondences[0].feature, correspondences[1].feature,
        correspondences[2].feature, correspondences[3].feature};
    const Vector3d world_points[4] = {
        correspondences[0].world_point, correspondences[1].world_point,
        correspondences[2].world_point, correspondences[3].world_point};

    std::vector<Matrix3d> rotations;
    std::vector<Vector3d> translations;
    std::vector<double> radial_distortions;
    std::vector<double> focal_lenghts;

    if (!FourPointsPoseFocalLengthRadialDistortion(
            features, world_points, meta_data_.max_focal_length,
            meta_data_.min_focal_length, meta_data_.max_radial_distortion,
            meta_data_.min_radial_distortion, &rotations, &translations,
            &radial_distortions, &focal_lenghts))
      return false;

    absolute_poses->resize(rotations.size());
    for (int i = 0; i < rotations.size(); ++i) {
      (*absolute_poses)[i].radial_distortion = radial_distortions[i];
      (*absolute_poses)[i].focal_length = focal_lenghts[i];
      (*absolute_poses)[i].rotation = rotations[i];
      (*absolute_poses)[i].translation = translations[i];
    }

    return rotations.size() > 0;
  }

  // The error for a correspondences given an absolute pose. This is the squared
  // reprojection error.
  double Error(const FeatureCorrespondence2D3D& correspondence,
               const RadialDistUncalibratedAbsolutePose& absolute_pose) const {
    // undistort the feature with the estimated radial distortion parameter
    // project der world point with the given focal length and
    // compare it to the undistorted image point
    Matrix3d K =
        Vector3d(absolute_pose.focal_length, absolute_pose.focal_length, 1.0)
            .asDiagonal();
    if (absolute_pose.translation[2] < 0.0) {
        return 1.0e10;
    }
    Vector3d reproj_pt = (absolute_pose.rotation * correspondence.world_point +
                          absolute_pose.translation);
    const Eigen::Vector2d reproj_pt_2d = (K * reproj_pt).hnormalized();
    Eigen::Vector2d distorted_point;
    DistortPoint(reproj_pt_2d, absolute_pose.radial_distortion,
                 &distorted_point);

    return (distorted_point - correspondence.feature).squaredNorm();
  }

  void SetMetadata(RadialDistUncalibratedAbsolutePoseMetaData meta_data) {
    meta_data_ = meta_data;
  }

 private:
  RadialDistUncalibratedAbsolutePoseMetaData meta_data_;

  DISALLOW_COPY_AND_ASSIGN(RadialDistUncalibratedAbsolutePoseEstimator);
};

}  // namespace

bool EstimateRadialDistUncalibratedAbsolutePose(
    const RansacParameters& ransac_params, const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    const RadialDistUncalibratedAbsolutePoseMetaData& meta_data,
    RadialDistUncalibratedAbsolutePose* absolute_pose,
    RansacSummary* ransac_summary) {
  RadialDistUncalibratedAbsolutePoseEstimator absolute_pose_estimator;
  absolute_pose_estimator.SetMetadata(meta_data);

  std::unique_ptr<
      SampleConsensusEstimator<RadialDistUncalibratedAbsolutePoseEstimator> >
      ransac = CreateAndInitializeRansacVariant(ransac_type, ransac_params,
                                                absolute_pose_estimator);
  // Estimate the absolute pose.
  const bool success = ransac->Estimate(normalized_correspondences,
                                        absolute_pose, ransac_summary);

  return success;
}

}  // namespace theia


#pragma once

#include <Eigen/Dense>
#include <vector>

#include "../flux_worker.hpp"

namespace boltzmann {
namespace impl_mls {

/**
 * @brief SpecularReflection
 *
 * velocity part (in Lagrange basis)
 *
 * upper hemisphere: outflow
 * lower hemisphere: inflow
 *
 */
class SpecularReflection : public boltzmann::impl::flux_worker
{
  using boltzmann::impl::flux_worker::vec_t;
  using boltzmann::impl::flux_worker::mat_t;

 public:
  SpecularReflection(const vec_t& hermite_weights, const vec_t& hermite_nodes)
      : w_(hermite_weights)
      , x_(hermite_nodes)
  {
    /* empty */
  }

  /**
   *
   *
   * @param out Ordering: out(i,j) = f(x_i, y_j)
   * \remark{Also see documentation of @ref H2N.}
   * @param in
   */
  virtual void apply(mat_t& out, const mat_t& in, const dealii::Point<2>& dummy) const;

 private:
  const vec_t w_;
  const vec_t x_;
};

void
SpecularReflection::apply(mat_t& out, const mat_t& in, const dealii::Point<2>& dummy) const
{
  AssertDimension(in.rows(), out.rows());
  AssertDimension(in.cols(), out.cols());
  AssertDimension(in.cols(), w_.size());

  int N = w_.size();

  int nhalf = N / 2;

  // reflection
  for (int j = 0; j < N; ++j) {
    for (int i = 0; i < N; ++i) {
      if (i < nhalf)
        out(i, j) = in(N - 1 - i, j);
      else
        out(i, j) = in(i, j);
    }
  }

  // multiply with weights
  for (int j = 0; j < N; ++j) {
    for (int i = 0; i < N; ++i) {
      out(i, j) *= x_[i];
    }
  }
}

}  // end namespace impl
}  // end namespace boltzmann


#pragma once

#include <merely3d/types.hpp>
#include <merely3d/color.hpp>
#include <Eigen/Dense>

namespace merely3d
{
    struct Rectangle
    {
        Rectangle() : extents(Eigen::Vector2f::Zero()) {}
        explicit Rectangle(const Eigen::Vector2f & extents) : extents(extents) {}
        Rectangle(float x_extent, float y_extent)
            : extents(Eigen::Vector2f(x_extent, y_extent)) {}

        UnalignedVector2f extents;
    };

    struct Box
    {
        Box() : extents(Eigen::Vector3f::Zero()) {}
        explicit Box(const Eigen::Vector3f & extents) : extents(extents) {}
        Box(float x_extent, float y_extent, float z_extent)
            : extents(Eigen::Vector3f(x_extent, y_extent, z_extent)) {}

        Eigen::Vector3f extents;
    };

    struct Sphere
    {
        Sphere() : radius(1.0) {}
        explicit Sphere(float radius) : radius(radius) {}

        float radius;
    };

    struct Line
    {
        Line(const Eigen::Vector3f & from, const Eigen::Vector3f & to, const Color & color = blue())
                : from(from), to(to), color(color) {}

        Eigen::Vector3f from;
        Eigen::Vector3f to;
        Color           color;
    };

    static const Color DEFAULT_PARTICLE_COLOR = Color(0.1, 0.1, 0.7);
    static float DEFAULT_PARTICLE_RADIUS = 0.2;

    struct Particle
    {
        Particle() : Particle(Eigen::Vector3f::Zero()) {}
        Particle(const Eigen::Vector3f & position,
                 float radius = DEFAULT_PARTICLE_RADIUS,
                 const Color & color = DEFAULT_PARTICLE_COLOR)
            : position(position), color(color), radius(radius) {}
        Particle(float x, float y, float z,
                 float radius = DEFAULT_PARTICLE_RADIUS,
                 const Color & color = DEFAULT_PARTICLE_COLOR)
            : Particle(Eigen::Vector3f(x, y, z), radius, color)
        {}

        Eigen::Vector3f position;
        Color           color;
        float           radius;

        Particle with_radius(float new_radius) const
        {
            return Particle(position, new_radius, color);
        }

        Particle with_position(const Eigen::Vector3f & new_position) const
        {
            return Particle(new_position, radius, color);
        }

        Particle with_position(float x, float y, float z) const
        {
            return Particle(x, y, z, radius, color);
        }

        Particle with_color(const Color & new_color) const
        {
            return Particle(position, radius, new_color);
        }
    };
}