#include "gcransac_python.h"
#include <vector>
#include <thread>
#include "utils.h"
#include <opencv2/core/core.hpp>
#include <Eigen/Eigen>

#include "GCRANSAC.h"
#include "flann_neighborhood_graph.h"
#include "grid_neighborhood_graph.h"
#include "uniform_sampler.h"
#include "prosac_sampler.h"
#include "progressive_napsac_sampler.h"
#include "fundamental_estimator.h"
#include "homography_estimator.h"
#include "essential_estimator.h"
#include "preemption_sprt.h"

#include "solver_fundamental_matrix_seven_point.h"
#include "solver_fundamental_matrix_eight_point.h"
#include "solver_homography_four_point.h"
#include "solver_essential_matrix_five_point_stewenius.h"

#include <ctime>
#include <sys/types.h>
#include <sys/stat.h>

using namespace gcransac;

int find6DPose_(std::vector<double>& imagePoints,
	std::vector<double>& worldPoints,
	std::vector<bool>& inliers,
	std::vector<double> &pose,
	double spatial_coherence_weight,
	double threshold,
	double conf,
	int max_iters,
	bool use_sprt)
{
	size_t num_tents = imagePoints.size() / 2;
	cv::Mat points(num_tents, 5, CV_64F);
	size_t iterations = 0;
	for (size_t i = 0; i < num_tents; ++i) {
		points.at<double>(i, 0) = imagePoints[2 * i];
		points.at<double>(i, 1) = imagePoints[2 * i + 1];
		points.at<double>(i, 2) = worldPoints[3 * i];
		points.at<double>(i, 3) = worldPoints[3 * i + 1];
		points.at<double>(i, 4) = worldPoints[3 * i + 2];
	}

	const size_t cell_number_in_neighborhood_graph_ = 8;
	neighborhood::FlannNeighborhoodGraph neighborhood1(&points, 20);

	// Apply Graph-cut RANSAC
	utils::DefaultPnPEstimator estimator;
	Pose6D model;

	// Initialize the samplers
	sampler::UniformSampler main_sampler(&points);  // The main sampler is used inside the local optimization
	sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization

	// Checking if the samplers are initialized successfully.
	if (!main_sampler.isInitialized() ||
		!local_optimization_sampler.isInitialized())
	{
		fprintf(stderr, "One of the samplers is not initialized successfully.\n");
		return 0;
	}

	utils::RANSACStatistics statistics;

	if (use_sprt)
	{
		// Initializing SPRT test
		preemption::SPRTPreemptiveVerfication<utils::DefaultPnPEstimator> preemptive_verification(
			points,
			estimator);

		GCRANSAC<utils::DefaultPnPEstimator,
			neighborhood::FlannNeighborhoodGraph,
			MSACScoringFunction<utils::DefaultPnPEstimator>,
			preemption::SPRTPreemptiveVerfication<utils::DefaultPnPEstimator>> gcransac;
		gcransac.setFPS(-1); // Set the desired FPS (-1 means no limit)
		gcransac.settings.threshold = threshold; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball
		gcransac.settings.core_number = std::thread::hardware_concurrency(); // The number of parallel processes

		// Start GC-RANSAC
		gcransac.run(points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model,
			preemptive_verification);

		statistics = gcransac.getRansacStatistics();
	}
	else
	{
		GCRANSAC<utils::DefaultPnPEstimator,
			neighborhood::FlannNeighborhoodGraph> gcransac;
		gcransac.setFPS(-1); // Set the desired FPS (-1 means no limit)
		gcransac.settings.threshold = threshold; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball
		gcransac.settings.core_number = std::thread::hardware_concurrency(); // The number of parallel processes

		// Start GC-RANSAC
		gcransac.run(points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model);

		statistics = gcransac.getRansacStatistics();
	}

	inliers.resize(num_tents);

	const int num_inliers = statistics.inliers.size();
	for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) {
		inliers[pt_idx] = 0;
	}

	for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) {
		inliers[statistics.inliers[pt_idx]] = 1;
	}

	pose.resize(12);

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 4; j++) {
			pose[i * 4 + j] = (double)model.descriptor(i, j);
		}
	}
	return num_inliers;
}

int findFundamentalMatrix_(std::vector<double>& srcPts,
	std::vector<double>& dstPts,
	std::vector<bool>& inliers,
	std::vector<double>&F,
	int h1, int w1, int h2, int w2,
	double spatial_coherence_weight,
	double threshold,
	double conf,
	int max_iters,
	bool use_sprt)
{
	int num_tents = srcPts.size() / 2;
	cv::Mat points(num_tents, 4, CV_64F);
	int iterations = 0;
	for (int i = 0; i < num_tents; ++i) {
		points.at<double>(i, 0) = srcPts[2 * i];
		points.at<double>(i, 1) = srcPts[2 * i + 1];
		points.at<double>(i, 2) = dstPts[2 * i];
		points.at<double>(i, 3) = dstPts[2 * i + 1];
	}
	const size_t cell_number_in_neighborhood_graph_ = 8;
	neighborhood::GridNeighborhoodGraph neighborhood1(&points,
		w1 / static_cast<double>(cell_number_in_neighborhood_graph_),
		h1 / static_cast<double>(cell_number_in_neighborhood_graph_),
		w2 / static_cast<double>(cell_number_in_neighborhood_graph_),
		h2 / static_cast<double>(cell_number_in_neighborhood_graph_),
		cell_number_in_neighborhood_graph_);

	// Checking if the neighborhood graph is initialized successfully.
	if (!neighborhood1.isInitialized())
	{
		fprintf(stderr, "The neighborhood graph is not initialized successfully.\n");
		return 0;
	}

	// Calculating the maximum image diagonal to be used for setting the threshold
	// adaptively for each image pair.
	const double max_image_diagonal =
		sqrt(pow(MAX(w1, w2), 2) + pow(MAX(h1, h2), 2));

	// Apply Graph-cut RANSAC
	utils::DefaultFundamentalMatrixEstimator estimator;
	FundamentalMatrix model;

	// Initialize the samplers
	// The main sampler is used inside the local optimization
	sampler::ProgressiveNapsacSampler main_sampler(&points,
		{ 16, 8, 4, 2 },	// The layer of grids. The cells of the finest grid are of dimension
							// (source_image_width / 16) * (source_image_height / 16)  * (destination_image_width / 16)  (destination_image_height / 16), etc.
		estimator.sampleSize(), // The size of a minimal sample
		static_cast<double>(w1), // The width of the source image
		static_cast<double>(h1), // The height of the source image
		static_cast<double>(w2), // The width of the destination image
		static_cast<double>(h2));  // The height of the destination image
	sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization

	// Checking if the samplers are initialized successfully.
	if (!main_sampler.isInitialized() ||
		!local_optimization_sampler.isInitialized())
	{
		fprintf(stderr, "One of the samplers is not initialized successfully.\n");
		return 0;
	}

	utils::RANSACStatistics statistics;

	if (use_sprt)
	{
		// Initializing SPRT test
		preemption::SPRTPreemptiveVerfication<utils::DefaultFundamentalMatrixEstimator> preemptive_verification(
			points,
			estimator);

		GCRANSAC<utils::DefaultFundamentalMatrixEstimator,
			neighborhood::GridNeighborhoodGraph,
			MSACScoringFunction<utils::DefaultFundamentalMatrixEstimator>,
			preemption::SPRTPreemptiveVerfication<utils::DefaultFundamentalMatrixEstimator>> gcransac;
		gcransac.setFPS(-1); // Set the desired FPS (-1 means no limit)
		gcransac.settings.threshold = threshold; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball
		gcransac.settings.core_number = std::thread::hardware_concurrency(); // The number of parallel processes

		// Start GC-RANSAC
		gcransac.run(points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model,
			preemptive_verification);

		statistics = gcransac.getRansacStatistics();
	}
	else
	{
		GCRANSAC<utils::DefaultFundamentalMatrixEstimator, neighborhood::GridNeighborhoodGraph> gcransac;
		gcransac.settings.threshold = 0.0005 * threshold * max_image_diagonal; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball

		// Start GC-RANSAC
		gcransac.run(points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model);

		statistics = gcransac.getRansacStatistics();
	}

	printf("Inlier number = %d\n", static_cast<int>(statistics.inliers.size()));

	inliers.resize(num_tents);

	const int num_inliers = statistics.inliers.size();
	for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) {
		inliers[pt_idx] = 0;
	}

	for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) {
		inliers[statistics.inliers[pt_idx]] = 1;
	}

	F.resize(9);

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			F[i * 3 + j] = (double)model.descriptor(i, j);
		}
	}
	return num_inliers;
}

int findEssentialMatrix_(std::vector<double>& srcPts,
	std::vector<double>& dstPts,
	std::vector<bool>& inliers,
	std::vector<double>&E,
	std::vector<double>& src_K,
	std::vector<double>& dst_K,
	int h1, int w1, int h2, int w2,
	double spatial_coherence_weight,
	double threshold,
	double conf,
	int max_iters,
	bool use_sprt)
{
	int num_tents = srcPts.size() / 2;
	cv::Mat points(num_tents, 4, CV_64F);
	int iterations = 0;
	for (int i = 0; i < num_tents; ++i) {
		points.at<double>(i, 0) = srcPts[2 * i];
		points.at<double>(i, 1) = srcPts[2 * i + 1];
		points.at<double>(i, 2) = dstPts[2 * i];
		points.at<double>(i, 3) = dstPts[2 * i + 1];
	}
	const size_t cell_number_in_neighborhood_graph_ = 8;
	neighborhood::GridNeighborhoodGraph neighborhood1(&points,
		w1 / static_cast<double>(cell_number_in_neighborhood_graph_),
		h1 / static_cast<double>(cell_number_in_neighborhood_graph_),
		w2 / static_cast<double>(cell_number_in_neighborhood_graph_),
		h2 / static_cast<double>(cell_number_in_neighborhood_graph_),
		cell_number_in_neighborhood_graph_);

	// Checking if the neighborhood graph is initialized successfully.
	if (!neighborhood1.isInitialized())
	{
		fprintf(stderr, "The neighborhood graph is not initialized successfully.\n");
		return 0;
	}

	// Calculating the maximum image diagonal to be used for setting the threshold
	// adaptively for each image pair.
	const double max_image_diagonal =
		sqrt(pow(MAX(w1, w2), 2) + pow(MAX(h1, h2), 2));

	Eigen::Matrix3d intrinsics_src,
		intrinsics_dst;

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			intrinsics_src(i, j) = src_K[i * 3 + j];
		}
	}

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			intrinsics_dst(i, j) = dst_K[i * 3 + j];
		}
	}

	cv::Mat normalized_points(points.size(), CV_64F);
	utils::normalizeCorrespondences(points,
		intrinsics_src,
		intrinsics_dst,
		normalized_points);

	// Apply Graph-cut RANSAC
	utils::DefaultEssentialMatrixEstimator estimator(intrinsics_src,
		intrinsics_dst);
	EssentialMatrix model;

	// Initialize the samplers
	// The main sampler is used inside the local optimization
	sampler::ProgressiveNapsacSampler main_sampler(&points,
		{ 16, 8, 4, 2 },	// The layer of grids. The cells of the finest grid are of dimension
							// (source_image_width / 16) * (source_image_height / 16)  * (destination_image_width / 16)  (destination_image_height / 16), etc.
		estimator.sampleSize(), // The size of a minimal sample
		static_cast<double>(w1), // The width of the source image
		static_cast<double>(h1), // The height of the source image
		static_cast<double>(w2), // The width of the destination image
		static_cast<double>(h2));  // The height of the destination image
	sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization

	// Checking if the samplers are initialized successfully.
	if (!main_sampler.isInitialized() ||
		!local_optimization_sampler.isInitialized())
	{
		fprintf(stderr, "One of the samplers is not initialized successfully.\n");
		return 0;
	}

	utils::RANSACStatistics statistics;

	if (use_sprt)
	{
		// Initializing SPRT test
		preemption::SPRTPreemptiveVerfication<utils::DefaultEssentialMatrixEstimator> preemptive_verification(
			points,
			estimator);

		GCRANSAC<utils::DefaultEssentialMatrixEstimator,
			neighborhood::GridNeighborhoodGraph,
			MSACScoringFunction<utils::DefaultEssentialMatrixEstimator>,
			preemption::SPRTPreemptiveVerfication<utils::DefaultEssentialMatrixEstimator>> gcransac;
		gcransac.settings.threshold = threshold; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball

		// Start GC-RANSAC
		gcransac.run(points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model,
			preemptive_verification);

		statistics = gcransac.getRansacStatistics();
	}
	else
	{
		GCRANSAC<utils::DefaultEssentialMatrixEstimator, neighborhood::GridNeighborhoodGraph> gcransac;
		gcransac.settings.threshold = threshold; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball

		// Start GC-RANSAC
		gcransac.run(normalized_points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model);

		statistics = gcransac.getRansacStatistics();
	}

	inliers.resize(num_tents);

	const int num_inliers = statistics.inliers.size();
	for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) {
		inliers[pt_idx] = 0;

	}
	for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) {
		inliers[statistics.inliers[pt_idx]] = 1;
	}

	E.resize(9);

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			E[i * 3 + j] = (double)model.descriptor(i, j);
		}
	}
	return num_inliers;
}

int findHomography_(std::vector<double>& srcPts,
	std::vector<double>& dstPts,
	std::vector<bool>& inliers,
	std::vector<double>& H,
	int h1, int w1, int h2, int w2,
	double spatial_coherence_weight,
	double threshold,
	double conf,
	int max_iters,
	bool use_sprt)
{
	const size_t cell_number_in_neighborhood_graph_ = 8;

	int num_tents = srcPts.size() / 2;
	cv::Mat points(num_tents, 4, CV_64F);
	int iterations = 0;
	for (int i = 0; i < num_tents; ++i) {
		points.at<double>(i, 0) = srcPts[2 * i];
		points.at<double>(i, 1) = srcPts[2 * i + 1];
		points.at<double>(i, 2) = dstPts[2 * i];
		points.at<double>(i, 3) = dstPts[2 * i + 1];
	}

	neighborhood::GridNeighborhoodGraph neighborhood1(&points,
		w1 / static_cast<double>(cell_number_in_neighborhood_graph_),
		h1 / static_cast<double>(cell_number_in_neighborhood_graph_),
		w2 / static_cast<double>(cell_number_in_neighborhood_graph_),
		h2 / static_cast<double>(cell_number_in_neighborhood_graph_),
		//		source_image.cols / static_cast<double>(cell_number_in_neighborhood_graph_),
		//		source_image.rows / static_cast<double>(cell_number_in_neighborhood_graph_),
		//		destination_image.cols / static_cast<double>(cell_number_in_neighborhood_graph_),
		//		destination_image.rows / static_cast<double>(cell_number_in_neighborhood_graph_),
		cell_number_in_neighborhood_graph_);

	// Checking if the neighborhood graph is initialized successfully.
	if (!neighborhood1.isInitialized())
	{
		fprintf(stderr, "The neighborhood graph is not initialized successfully.\n");
		return 0;
	}

	// Calculating the maximum image diagonal to be used for setting the threshold
	// adaptively for each image pair.
	const double max_image_diagonal =
		sqrt(pow(MAX(w1, w2), 2) + pow(MAX(h1, h2), 2));

	utils::DefaultHomographyEstimator estimator;
	Homography model;

	sampler::ProgressiveNapsacSampler main_sampler(&points,
		{ 16, 8, 4, 2 },	// The layer of grids. The cells of the finest grid are of dimension
							// (source_image_width / 16) * (source_image_height / 16)  * (destination_image_width / 16)  (destination_image_height / 16), etc.
		estimator.sampleSize(), // The size of a minimal sample
		static_cast<double>(w1), // The width of the source image
		static_cast<double>(h1), // The height of the source image
		static_cast<double>(w2), // The width of the destination image
		static_cast<double>(h2),  // The height of the destination image
		0.5); // The length (i.e., 0.5 * <point number> iterations) of fully blending to global sampling

	sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization

	// Checking if the samplers are initialized successfully.
	if (!main_sampler.isInitialized() ||
		!local_optimization_sampler.isInitialized())
	{
		fprintf(stderr, "One of the samplers is not initialized successfully.\n");
		return 0;
	}

	utils::RANSACStatistics statistics;

	if (use_sprt)
	{
		// Initializing SPRT test
		preemption::SPRTPreemptiveVerfication<utils::DefaultHomographyEstimator> preemptive_verification(
			points,
			estimator);

		GCRANSAC<utils::DefaultHomographyEstimator,
			neighborhood::GridNeighborhoodGraph,
			MSACScoringFunction<utils::DefaultHomographyEstimator>,
			preemption::SPRTPreemptiveVerfication<utils::DefaultHomographyEstimator>> gcransac;
		gcransac.settings.threshold = threshold; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball

		// Start GC-RANSAC
		gcransac.run(points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model,
			preemptive_verification);

		statistics = gcransac.getRansacStatistics();
	}
	else
	{
		GCRANSAC<utils::DefaultHomographyEstimator, neighborhood::GridNeighborhoodGraph> gcransac;
		gcransac.settings.threshold = threshold; // The inlier-outlier threshold
		gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term
		gcransac.settings.confidence = conf; // The required confidence in the results
		gcransac.settings.max_local_optimization_number = 50; // The maximum number of local optimizations
		gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations
		gcransac.settings.min_iteration_number = 50; // The minimum number of iterations
		gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball

		// Start GC-RANSAC
		gcransac.run(points,
			estimator,
			&main_sampler,
			&local_optimization_sampler,
			&neighborhood1,
			model);

		statistics = gcransac.getRansacStatistics();
	}

	H.resize(9);

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			H[i * 3 + j] = model.descriptor(i, j);
		}
	}

	inliers.resize(num_tents);

	const int num_inliers = statistics.inliers.size();
	for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) {
		inliers[pt_idx] = 0;

	}
	for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) {
		inliers[statistics.inliers[pt_idx]] = 1;
	}

	return num_inliers;
}


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

// AXPY
template<>
void caffe_axpy<float>(const int_tp n, const float alpha, const float* X,
                       float* Y) {
  cblas_saxpy(n, alpha, X, int_tp(1), Y, int_tp(1));
}
template<>
void caffe_axpy<double>(const int_tp n, const double alpha, const double* X,
                        double* Y) {
  cblas_daxpy(n, alpha, X, int_tp(1), Y, int_tp(1));
}
template<typename Dtype>
void caffe_axpy(const int_tp n, const Dtype alpha,
                         const Dtype* X, Dtype* Y) {
  for (int_tp i = 0; i < n; ++i) {
    Y[i] += alpha * X[i];
  }
}
template void caffe_axpy<half_fp>(const int_tp n, const half_fp alpha,
                         const half_fp* X, half_fp* Y);
template void caffe_axpy<uint8_t>(const int_tp n, const uint8_t alpha,
                         const uint8_t* X, uint8_t* Y);
template void caffe_axpy<uint16_t>(const int_tp n, const uint16_t alpha,
                         const uint16_t* X, uint16_t* Y);
template void caffe_axpy<uint32_t>(const int_tp n, const uint32_t alpha,
                         const uint32_t* X, uint32_t* Y);
template void caffe_axpy<uint64_t>(const int_tp n, const uint64_t alpha,
                         const uint64_t* X, uint64_t* Y);

// AXPBY
template<>
void caffe_axpby<half_fp>(const int_tp n,
                        const half_fp alpha, const half_fp* X,
                        const half_fp beta, half_fp* Y) {
  cblas_haxpby(n, alpha, X, int_tp(1), beta, Y, int_tp(1));
}
template<>
void caffe_axpby<float>(const int_tp n, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(n, alpha, X, int_tp(1), beta, Y, int_tp(1));
}
template<>
void caffe_axpby<double>(const int_tp n, const double alpha,
                             const double* X, const double beta, double* Y) {
  cblas_daxpby(n, alpha, X, int_tp(1), beta, Y, int_tp(1));
}
template<typename Dtype>
void caffe_axpby(const int_tp n, const Dtype alpha, const Dtype* X,
                 const Dtype beta, Dtype* Y) {
  for (int_tp i = 0; i < n; ++i) {
    Y[i] = alpha * X[i] + beta * Y[i];
  }
}
template void caffe_axpby<half_fp>(const int_tp n, const half_fp alpha,
                         const half_fp* X, const half_fp beta, half_fp* Y);
template void caffe_axpby<uint8_t>(const int_tp n, const uint8_t alpha,
                         const uint8_t* X, const uint8_t beta, uint8_t* Y);
template void caffe_axpby<uint16_t>(const int_tp n, const uint16_t alpha,
                         const uint16_t* X, const uint16_t beta, uint16_t* Y);
template void caffe_axpby<uint32_t>(const int_tp n, const uint32_t alpha,
                         const uint32_t* X, const uint32_t beta, uint32_t* Y);
template void caffe_axpby<uint64_t>(const int_tp n, const uint64_t alpha,
                         const uint64_t* X, const uint64_t beta, uint64_t* Y);

// DOT
template<typename Dtype>
Dtype caffe_strided_dot(const int_tp n,
                                const Dtype* x, const int_tp incx,
                                const Dtype* y, const int_tp incy) {
  half_fp sum = 0;
  for (int_tp i = 0; i < n; i++) {
    sum += x[i * incx] * y[i * incy];
  }
  return sum;
}

template half_fp caffe_strided_dot(const int_tp n,
                                   const half_fp* x, const int_tp incx,
                                   const half_fp* y, const int_tp incy);
template uint8_t caffe_strided_dot(const int_tp n,
                                  const uint8_t* x, const int_tp incx,
                                  const uint8_t* y, const int_tp incy);
template uint16_t caffe_strided_dot(const int_tp n,
                                   const uint16_t* x, const int_tp incx,
                                   const uint16_t* y, const int_tp incy);
template uint32_t caffe_strided_dot(const int_tp n,
                                   const uint32_t* x, const int_tp incx,
                                   const uint32_t* y, const int_tp incy);
template uint64_t caffe_strided_dot(const int_tp n,
                                   const uint64_t* x, const int_tp incx,
                                   const uint64_t* y, const int_tp incy);

template<>
float caffe_strided_dot<float>(const int_tp n, const float* X,
                               const int_tp incx, const float* Y,
                               const int_tp incy) {
  return cblas_sdot(n, X, incx, Y, incy);
}

template<>
double caffe_strided_dot<double>(const int_tp n, const double* X,
                                 const int_tp incx, const double* Y,
                                 const int_tp incy) {
  return cblas_ddot(n, X, incx, Y, incy);
}

template<typename Dtype>
Dtype caffe_dot(const int_tp n, const Dtype* X, const Dtype* Y) {
  Dtype r = Dtype(0);
  for (size_t i = 0; i < n; i++) {
    r += X[i] * Y[i];
  }
  return r;
}

template half_fp caffe_dot<half_fp>(const int_tp n,
                               const half_fp* X, const half_fp* Y);
template uint8_t caffe_dot<uint8_t>(const int_tp n, const uint8_t* X,
                              const uint8_t* Y);
template uint16_t caffe_dot<uint16_t>(const int_tp n, const uint16_t* X,
                              const uint16_t* Y);
template uint32_t caffe_dot<uint32_t>(const int_tp n, const uint32_t* X,
                              const uint32_t* Y);
template uint64_t caffe_dot<uint64_t>(const int_tp n, const uint64_t* X,
                              const uint64_t* Y);

template<>
float caffe_dot(const int_tp n, const float* X, const float* Y) {
  return caffe_strided_dot(n, X, int_tp(1), Y, int_tp(1));
}
template<>
double caffe_dot(const int_tp n, const double* X, const double* Y) {
  return caffe_strided_dot(n, X, int_tp(1), Y, int_tp(1));
}

// ASUM
template<>
half_fp caffe_asum<half_fp>(const int_tp n, const half_fp* x) {
  half_fp sum = 0;
  for (int_tp i = 0; i < n; i++)
    sum += fabs(x[i]);
  return sum;
}
template<>
float caffe_asum<float>(const int_tp n, const float* x) {
  return cblas_sasum(n, x, int_tp(1));
}
template<>
double caffe_asum<double>(const int_tp n, const double* x) {
  return cblas_dasum(n, x, int_tp(1));
}
template<typename Dtype>
Dtype caffe_asum(const int_tp n, const Dtype* x) {
  Dtype sum = 0;
  for (int_tp i = 0; i < n; i++)
    sum += x[i];
  return sum;
}
template uint8_t caffe_asum<uint8_t>(const int_tp n, const uint8_t* x);
template uint16_t caffe_asum<uint16_t>(const int_tp n, const uint16_t* x);
template uint32_t caffe_asum<uint32_t>(const int_tp n, const uint32_t* x);
template uint64_t caffe_asum<uint64_t>(const int_tp n, const uint64_t* x);

// SCALE
template<typename Dtype>
void caffe_scale(const int_tp n, const Dtype alpha, const Dtype *X,
                 Dtype* Y) {
  for (int_tp i = 0; i < n; i++) {
    Y[i] = X[i];
  }
  caffe_scal(n, alpha, Y);
}

template void caffe_scale<half_fp>(const int_tp n, const half_fp alpha,
                                   const half_fp *X, half_fp* Y);
template void caffe_scale<uint8_t>(const int_tp n, const uint8_t alpha,
                                   const uint8_t *X, uint8_t* Y);
template void caffe_scale<uint16_t>(const int_tp n, const uint16_t alpha,
                                   const uint16_t *X, uint16_t* Y);
template void caffe_scale<uint32_t>(const int_tp n, const uint32_t alpha,
                                   const uint32_t *X, uint32_t* Y);
template void caffe_scale<uint64_t>(const int_tp n, const uint64_t alpha,
                                   const uint64_t *X, uint64_t* Y);

template<>
void caffe_scale<float>(const int_tp n, const float alpha, const float *X,
                            float* Y) {
  cblas_scopy(n, X, int_tp(1), Y, int_tp(1));
  cblas_sscal(n, alpha, Y, int_tp(1));
}

template<>
void caffe_scale<double>(const int_tp n, const double alpha,
                             const double *X, double* Y) {
  cblas_dcopy(n, X, int_tp(1), Y, int_tp(1));
  cblas_dscal(n, alpha, Y, int_tp(1));
}

// SCAL
template<>
void caffe_scal<float>(const int_tp n, const float alpha, float *X) {
  cblas_sscal(n, alpha, X, int_tp(1));
}
template<>
void caffe_scal<double>(const int_tp n, const double alpha, double *X) {
  cblas_dscal(n, alpha, X, int_tp(1));
}
template<typename Dtype>
void caffe_scal(const int_tp n, const Dtype alpha, Dtype *X) {
  for (int_tp i = 0; i < n; ++i) {
    X[i] *= alpha;
  }
}
template void caffe_scal<half_fp>(const int_tp n, const half_fp alpha,
                                  half_fp *X);
template void caffe_scal<uint8_t>(const int_tp n, const uint8_t alpha,
                                 uint8_t *X);
template void caffe_scal<uint16_t>(const int_tp n, const uint16_t alpha,
                                  uint16_t *X);
template void caffe_scal<uint32_t>(const int_tp n, const uint32_t alpha,
                                  uint32_t *X);
template void caffe_scal<uint64_t>(const int_tp n, const uint64_t alpha,
                                  uint64_t *X);

}  // namespace caffe


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file fxtarfengine.hpp
    \brief Base class for FX Tarf engines
*/

#ifndef quantlib_pricingengines_fxtarf_hpp
#define quantlib_pricingengines_fxtarf_hpp

#include <ql/event.hpp>
#include <ql/experimental/fx/fxtarf.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

class FxTarfEngine : public FxTarf::engine {
  public:
    //! constructor
    FxTarfEngine(const Handle<YieldTermStructure> discount);

    void calculate() const;

  protected:
    const Handle<YieldTermStructure> discount_;
    mutable Real unsettledAmountNpv_;
};

} // namespace QuantLib
#endif


#ifndef IP_AUX_HH
#define IP_AUX_HH

#include <memory>
#include "ipfunctional.hh"
#include "fem/variables.hh"
#include "fem/functional_aux.hh"
#include "dune_bridge.hh"
#include "newton_damped.hh"
#include "homotopy_base.hh"
#include "fem/functional_manipulation.hh"
#include <limits.h>
#include <boost/timer/timer.hpp>
#include "fem/special_quadrature.hh"

/**
 * @file
 * @brief Much of the implementation for barrier methods
 * @author Anton Schiela
 *
 */
namespace Bridge{
/// Linearization for Constrained Optimization with Barrier methods
/** Combines the Functional to be minimized and appropriate barrier terms
 *  Barrier terms are assembled separately via a different type of quadrature rule.
 */
template<class BarrierFunctional,
         class VectorImpl,
         class ImageImpl=VectorImpl,
         class QuadRule =  TrapezoidalRule<typename BarrierFunctional::AnsatzVars::Grid::ctype, BarrierFunctional::AnsatzVars::Grid::dimension> >
class IPLinearization
{
public:
  typedef BarrierFunctional IPFunctional;
  typedef typename BarrierFunctional::AnsatzVars::Grid Grid;
  typedef typename Grid::LeafGridView GridView;
  typedef VectorImpl DomainElement;
  typedef ImageImpl ImageElement;
  typedef typename BarrierFunctional::Scalar Scalar;

  typedef KaskadeLinearization<typename BarrierFunctional::OptimalityFunctional, VectorImpl> Unconstrained;
  typedef KaskadeLinearization<BarrierFunctional, VectorImpl, QuadRule> Barrier;
  typedef Dune::LinearOperator<DomainElement, ImageElement> OperatorType;

  IPLinearization(BarrierFunctional const& fu, DomainElement const& x_)
    : blin(fu,x_), vlin(fu.unconstrainedFunctional,x_), bfun(fu), prepared(false)
  {
    flush();
    bfun.prepareConstraintsCache(vlin.getOriginImpl());
    prepared=true;
  }

    void precompute() {
    if(!prepared)
    {
      bfun.prepareConstraintsCache(vlin.getOriginImpl());
      prepared=true;
    }
      vlin.precompute();
      blin.precompute();
    }

/// Return gap parameter
  double getMu() const {return bfun.mu; }

  int cols(int cbegin, int cend) const { return vlin.cols(cbegin, cend);}
  int rows(int cbegin, int cend) const { return vlin.rows(cbegin, cend);}

  void getMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin, int rend, int colbegin, int colend) const
  {
    matb.resize(0);
    if(!prepared)
    {
      bfun.prepareConstraintsCache(vlin.getOriginImpl());
      prepared=true;
    }
    vlin.getMatrixBlocks(mat, rbegin, rend, colbegin, colend);
    blin.getMatrixBlocks(matb, rbegin, rend, colbegin, colend);
    mat+=matb;
  }
  void getRHSBlocks(std::vector<Scalar>& rhs, int rbegin, int rend) const
  {
    vlin.getRHSBlocks(rhs, rbegin, rend);
    if(BarrierFunctional::parameterLin!=1)
    {
      if(!prepared)
      {
        bfun.prepareConstraintsCache(vlin.getOriginImpl());
        prepared=true;
      }
      blin.getRHSBlocks(rhsb, rbegin, rend);
      int iend=std::min(rhs.size(),rhsb.size());
      if(BarrierFunctional::parameterLin==0) for(int i=0; i<iend;++i)  rhs[i]+=rhsb[i];
      if(BarrierFunctional::parameterLin==2) for(int i=0; i<iend;++i)  rhs[i]=rhsb[i];
    }
  }

  DomainElement const& getOriginImpl() const {return vlin.getOriginImpl(); }

  int nRowBlocks() const { return vlin.nRowBlocks(); }
  int nColBlocks() const { return vlin.nColBlocks(); }

  void flush() { vlin.flush(); blin.flush(); matb.resize(0); rhsb.resize(0); prepared=false; }

  void touch() { vlin.touch(); blin.touch(); }

  double getValue() const
  {
    if(!prepared)
    {
      bfun.prepareConstraintsCache(vlin.getOriginImpl());
      prepared=true;
    }
    return vlin.getValue()+blin.getValue();
  }

  double getL1norm() const
  {
    if(!prepared)
    {
      bfun.prepareConstraintsCache(vlin.getOriginImpl());
      prepared=true;
    }
    return vlin.getL1norm()+blin.getL1norm();
  }

  Barrier const& getBarrier() const {return blin;}
  Unconstrained const& getUnconstrained() const {return vlin;}

private:
  mutable MatrixAsTriplet<Scalar> matb;
  mutable std::vector<Scalar> rhsb;
  Barrier blin;
  Unconstrained vlin;
  BarrierFunctional const& bfun;
  mutable bool prepared;
};

///Traits class to choose the right Bridge::Linearization class
template<class Descr, class Functional, class BarrierFu, int paralin>
  class LinearizationTraits<VariableSet<Descr>, IPFunctional<Functional,BarrierFu, paralin> >
  {
    typedef IPFunctional<Functional,BarrierFu, paralin> Func;
  public:
    typedef IPLinearization<Func,typename Func::AnsatzVars::VariableSet> Linearization;
  };

}

template<class BarrierFunction, int yIdx>
struct FirstOrderEquation
{
  double H,r,mu,upper;
  template<class Cache, class Arg>
  void computeRHS(Cache const& cache,Arg const& arg,double const& trial)
  {
    r= cache.template d1<yIdx>(arg)+H*trial;
 }

  double operator()(double y) const { return H*y-BarrierFunction::db(mu,mu/(upper-y))-r; }
};

template<class BarrierFunction,int yIdx>
struct SecondOrderEquation
{
  double H,r,mu,upper;
  template<class Cache, class Arg>
  void computeRHS(Cache const& cache,Arg  const& arg,double const& trial)
  {
    r=cache.d0()+H*trial;
  }
  double operator()(double y) const { return H*y-BarrierFunction::b(mu,mu/(upper-y))-r; }
};

#include "algorithm/opt_aux/src/include/zeroin.cpp"

/// Pointwise damping strategy
template<template <class BF, int yIdx> class PolyEquation>
class PointwiseCorrection
{
public:
template<class DomainElement, class QuadraticModelBarrier, class UnconstrainedLinearizationAt>
void correct(DomainElement& tx, const DomainElement& x, QuadraticModelBarrier const& blin, UnconstrainedLinearizationAt const& ulin, double mu)
{
  using namespace boost::fusion;
  boost::timer::cpu_timer refTimer;

  typedef typename UnconstrainedLinearizationAt::Functional                Unconstrained;
  typedef typename Unconstrained::AnsatzVars::Spaces                       Spaces;
  typedef typename Unconstrained::Scalar                                       Scalar;

  int const yIdx  = Unconstrained::yIdx;
  int const ySIdx = boost::fusion::result_of::value_at_c<typename Unconstrained::AnsatzVars::Variables,yIdx>::type::spaceIndex;

  typedef typename result_of::template value_at_c<typename DomainElement::Functions, yIdx>::type::Space YSpace;
  int const dim = YSpace::dim;
  YSpace const& yspace(at_c<yIdx>(tx.data).space());

  if(Unconstrained::ConstraintsCache::template bounds<yIdx>::upper==false) return;

  typedef ShapeFunctionCache<typename YSpace::Grid,Scalar> SfCache;
  SfCache sfCache;
  typedef typename result_of::as_vector<typename result_of::transform<Spaces, GetEvaluators<SfCache> >::type>::type SFEvaluators;
  SFEvaluators evaluators(transform(tx.descriptions.spaces,GetEvaluators<SfCache>(&sfCache)));

  typename QuadraticModelBarrier::DomainCache bdomc(blin.template createDomainCache());
  typename Unconstrained::ConstraintsCache                                          uconc(ulin.getFunctional().createConstraintsCache(tx));
  typename Unconstrained::DomainCache         udomc(ulin.template createDomainCache(8));

  typedef typename QuadraticModelBarrier::Linearization::Functional::BarrierFunction BarrierFunction;

  VariationalArg<Scalar,dim> unitarg;
  unitarg.value=1.0;
  unitarg.gradient=0.0;

  PolyEquation<BarrierFunction,yIdx> pe;
  pe.mu = mu;

  int nnodes(0),maxiter(0),sumiter(0);
  double ratio(2.0);
  std::vector<int> visited(yspace.degreesOfFreedom(),0);

  std::cout << "PwD " << std::flush;

  typedef typename YSpace::Grid::LeafGridView GridView;

  typedef typename GridView::template Codim<0>::Iterator CellIterator;

  for (CellIterator ci=yspace.gridView().template begin<0>(); ci!=yspace.gridView().template end<0>(); ++ci)
  {

    moveEvaluatorsToCell(evaluators,*ci);
    bdomc.moveTo(*ci);
    uconc.moveTo(*ci);
    udomc.moveTo(*ci);

    std::vector<Dune::FieldVector<typename YSpace::Grid::ctype, dim> >const &  iNodes(at_c<ySIdx>(evaluators).shapeFunctions().interpolationNodes());

    for (int i=0; i<iNodes.size(); ++i)
    {
      int globalIdx=at_c<ySIdx>(evaluators).globalIndices()[i];
      if(!visited[globalIdx])
      {
        visited[globalIdx]++;
        double trial   = (*at_c<yIdx>(tx.data))[globalIdx][0];
        double iterate = (*at_c<yIdx>( x.data))[globalIdx][0];
        if(trial > iterate)
        {
          moveEvaluatorsToIntegrationPoint(evaluators,iNodes[i]);
          uconc.evaluateAt(iNodes[i],evaluators);
          pe.upper = uconc.upperbound();
          assert(iterate <= pe.upper);

          if(pe.upper < 1e30)
          {
            nnodes++;
            udomc.evaluateAt(iNodes[i],evaluators);
            bdomc.evaluateAt(iNodes[i],evaluators);

// Initialize Polynomial Equation
// H y+b'(y)=r
// H = Lyy (Hessian of the Lagrangian)
// r = b'(y_)+b''(y_)delta y+Lyy y_+

            pe.H     = udomc.template d2<yIdx,yIdx,dim>(unitarg,unitarg)[0][0];
            pe.computeRHS(bdomc,unitarg,trial);

            int iterations;
            double upperB = std::min(trial,pe.upper);
            double lowerB = (upperB+iterate)/2.0;

// Here the solution of the pointwise damping equation takes place

            double solution = zeroin(lowerB,upperB,pe,(upperB-lowerB)*1e-12+4*std::numeric_limits<Scalar>::epsilon()*(1+upperB),iterations,100);

            (*at_c<yIdx>(tx.data))[globalIdx][0] = solution;

// Statistics and Warnings

            maxiter = std::max(maxiter,iterations);
            sumiter += iterations;
            ratio=std::min(ratio,std::fabs((solution-iterate)/(trial-iterate)));

//             if(iterations >= 100)
//             {
//               std::cout << "Warning: too many iterations: " << pe.upper-solution << std::endl;
//               std::cout << "Interval: [" << upperB << "," << lowerB << "] " << solution << std::endl;
//               std::cout << "mu:" << pe.mu << " r:" << pe.r << " H:" << pe.H << std::endl;
//             }
//             if(std::fabs((solution-iterate)/(trial-iterate)) < 1e-3
//                || std::fabs((solution-iterate)/(trial-iterate) > 1.0
//                             && std::fabs(trial-iterate)>1e-12 )  )
//             {
//               std::cout << "Small ratio:" << std::fabs((solution-iterate)/(trial-iterate)) << std::endl;
//               std::cout << "Interval: [" << upperB << "," << lowerB << "] " << trial << " " << solution << std::endl;
//               std::cout << trial - iterate << "="<< trial-solution << "+" << solution - iterate << std::endl;
//               std::cout << "mu:" << pe.mu << " r:" << pe.r << " H:" << pe.H << std::endl;
//             }
          }
        }
      }
    }
  }
  std::cout << (double)(refTimer.elapsed().user)/1e9 << " sec.";
  std::cout << "MaxRatio: " << ratio << std::endl;
}
};

template<class Functional, class DomainElement,class Implementation=Bridge::IPLinearization<Functional, DomainElement> >
class PWDampingChart: public AbstractChart
{
  typedef typename Functional::OptimalityFunctional Unconstrained;
  typedef Functional Combined;

public:

  PWDampingChart(bool includeDuals_=true) :includeDuals(includeDuals_){}

private:

  void addPerturbation(AbstractFunctionSpaceElement & trialIterate,
                      AbstractFunctionSpaceElement const& correction,
                       AbstractLinearization const& lin,
                       std::vector<AbstractFunctionSpaceElement* > basis = std::vector<AbstractFunctionSpaceElement* >()) const
  {
    trialIterate = lin.getOrigin();

    if(includeDuals)
      trialIterate += correction;
    else
      for(int i=0; i<trialIterate.nComponents();++i)
        if(trialIterate.getRole(i)!="dual")
          trialIterate.axpy(1.0,correction,i);

    using namespace boost::fusion;
    DomainElement const& x=Bridge::getImpl<DomainElement>(lin.getOrigin());
    DomainElement& tx=Bridge::getImpl<DomainElement>(trialIterate);

    Bridge::Linearization<Implementation> const&  limpl(dynamic_cast<Bridge::Linearization<Implementation> const& >(lin));
    double mu = limpl.getLinImpl().getMu();

    LinearizationAt<Unconstrained> ulin(limpl.getLinImpl().getUnconstrained().getLinImpl().getFunctional(),tx);
    QuadraticModel<LinearizationAt<Combined>,false> blin(linearization(limpl.getLinImpl().getBarrier().getLinImpl().getFunctional(),x),
                                                         Bridge::getImpl<DomainElement>(correction));
    PointwiseCorrection<FirstOrderEquation> pc;

    pc.correct(tx, x, blin, ulin, mu);
  }
private:
  bool includeDuals;
};

/// Pointwise damping strategy
template<class DomainElement, class QBarrierModel, class ParameterModel, class LModel>
void corrTangent(DomainElement& tx,const DomainElement& x, QBarrierModel const& blin, ParameterModel const& plin, LModel const& ulin, double muLast, double mu)
  {
    using namespace boost::fusion;

    typedef typename LModel::Functional Unconstrained;
    typedef typename LModel::AnsatzVars::Spaces    Spaces;
    typedef typename LModel::AnsatzVars::IndexSet  IndexSet;
    typedef typename Unconstrained::Entity  Entity;
    typedef typename LModel::Scalar  Scalar;
    int const yIdx = Unconstrained::yIdx;
    typedef typename result_of::template value_at_c<typename DomainElement::Functions, yIdx>::type::Space YSpace;
    int const dim = YSpace::dim;

    double deltaMu = muLast-mu;

    if(Unconstrained::ConstraintsCache::template bounds<yIdx>::upper==0) return;

    YSpace const& yspace(at_c<yIdx>(tx.data).space());
    typename YSpace::Evaluator ysfs(yspace);

    typedef ShapeFunctionCache<typename YSpace::Grid,Scalar> SfCache;
    SfCache sfCache;

    typedef typename result_of::as_vector<typename result_of::transform<Spaces, GetEvaluators<SfCache> >::type>::type Evaluators;
    Evaluators evaluators(transform(tx.descriptions.spaces,GetEvaluators<SfCache>(&sfCache)));

    typename QBarrierModel::DomainCache bdc(blin.template createDomainCache());
    typename ParameterModel::DomainCache pdc(plin.template createDomainCache());
    typename Unconstrained::ConstraintsCache udc(ulin.getFunctional().createConstraintsCache(tx));

    VariationalArg<Scalar,dim> unitarg;
    unitarg.value[0]=1.0;
    for(int i=0; i < dim; ++i) unitarg.gradient[0][i]=0.0;

    typedef typename IndexSet::template Codim<0>::template Partition<Dune::All_Partition>::Iterator CellIterator;
    for (CellIterator ci=yspace.indexSet().template begin<0,Dune::All_Partition>();
         ci!=yspace.indexSet().template end<0,Dune::All_Partition>(); ++ci)
    {
      ysfs.moveTo(*ci);
      moveEvaluatorsToCell(evaluators,*ci);
      bdc.moveTo(*ci);
      pdc.moveTo(*ci);
      udc.moveTo(*ci);
      std::vector<Dune::FieldVector<typename YSpace::Grid::ctype, dim> >const &  iNodes(ysfs.shapeFunctions().interpolationNodes());
      FirstOrderEquation<typename QBarrierModel::Linearization::Functional::BarrierFunction,yIdx> pe;
      for (int i=0; i<iNodes.size(); ++i)
      {
        ysfs.evaluateAt(iNodes[i]);
        moveEvaluatorsToIntegrationPoint(evaluators,iNodes[i]);
        udc.evaluateAt(iNodes[i],evaluators);
        bdc.evaluateAt(iNodes[i],evaluators);
        pdc.evaluateAt(iNodes[i],evaluators);
        pe.aqp = udc.quadTermA();
        pe.aq=(bdc.template d1<yIdx,dim>(unitarg))[0]+deltaMu*(pdc.template d1<yIdx,dim>(unitarg))[0]+udc.quadTermB();
        pe.mu= mu;
        double yc2;
        double trial= -(*at_c<yIdx>(tx.data))[ysfs.globalIndices()[i]][0]+udc.upperbound();
        double iterate= -(*at_c<yIdx>(x.data))[ysfs.globalIndices()[i]][0]+udc.upperbound();
        double upper = std::max(trial,iterate);
        double lower = std::min(trial,iterate);
        int iterations;
        yc2=bisection(std::max(0.0,lower),upper,pe,upper*1e-6+4*std::numeric_limits<Scalar>::epsilon()*(1+upper),iterations);
        (*at_c<yIdx>(tx.data))[ysfs.globalIndices()[i]][0]= udc.upperbound()-yc2;
      }
    }
  }

struct StepEquation
{
  double k;   // order of predictor (k=0 : classical, k=1 : tangential)
  double err; // estimated iteration error of last step
  double eta; // order coefficent : (k=0 : slope, k=1 : curvature);
  double mu;  // homotopy parameter
  double alpha; // assumed negative exponent for model of slope of path (usually alpha = 1/2)
  double Theta; // desired contraction
  double omega; // aff. cov. Lipschitz constant for Newton's method
  double beta;  // assumed exponent for model of omega (usually beta = 1/2)
  double operator()(double sigma) const
{ return -(err+eta*std::pow(mu,k+1)*std::pow(sigma,-alpha-k)*std::pow(1-sigma,k+1)-Theta/omega*std::pow(sigma,beta)); }
};

template<class IPF, class DomainVector>
class InteriorPointTangentialPredictor : public HomotopyBase
{
  typedef typename IPF::OptimalityFunctional Unconstrained;
  typedef IPF Barrier;
  typedef Bridge::IPLinearization<IPF, DomainVector> Implementation;

public:
  InteriorPointTangentialPredictor(NewtonsMethod& n_, AbstractNorm const& norm_, InteriorPointParameters& p_,
                          AbstractLinearSolver& solver_, AbstractNorm const* normPlain_=0,
                          NewtonsMethod* finalsolver_=0) :
    HomotopyBase(n_, p_),
    norm(norm_),
    solver(solver_),
    stp(0),
    normPlain(normPlain_),
    finalsolver(finalsolver_)
  {
    if(!normPlain_) normPlain=&norm;
    if(!finalsolver_) finalsolver=&n_;
  }

  virtual void initialize() {
    iterate=trialIterate->clone();
    predictor=trialIterate->clone();
    tangent=trialIterate->clone();
    ptangent=trialIterate->clone();
    nNewton=0;
  };
  virtual void finalize() {};

  virtual double muOnSuccess(int step) {
    double b = pp.omega*pp.accuracyCorrector;
    double c = pp.eta*pp.omega*p.mu;
    double a = c+p.desiredContraction;
    double sigma = (b+sqrt(b*b+4*a*c))/(2*a);
    sigma = sigma*sigma;

    StepEquation f;
    f.k=1;
    f.err = pp.accuracyCorrector;
    f.eta = pp.eta;
    f.mu = p.mu;
    f.alpha = 0.5;
    f.Theta = p.desiredContraction;
    f.omega = pp.omega;
    f.beta = 0.5;
    int iter;
    double sig = bisection(1e-20,1.0-1e-15,f,1e-6,iter);
    sigma = sig;
    sigma = std::min(sigma,p.reductionFactorWorst);
    sigma = std::max(sigma,p.reductionFactorBest);
    if(step==2) sigma=p.reductionStart;

    return sigma*p.mu;
  };

  virtual double muOnFailure() {
    p.sigma=0.5+p.sigma/2.0;
    return p.mu*p.sigma;
  };

  virtual double muFinal() { return p.muFinal;};

  virtual void updateModelOfHomotopy()
  {
    *predictor -= *trialIterate;
    double n(norm(*predictor));
    *iterate *= 0.0;
     std::unique_ptr<AbstractFunctional> fuPtr(functional->getParameterLinFunctional(makePars(p.muTrial.value())));
     std::unique_ptr<AbstractLinearization> paraLinearization(fuPtr->getLinearization(*trialIterate));
     solver.solve(*tangent,*paraLinearization);
     double nD=norm(*tangent);
     double nDp=norm(*ptangent);

     if(stp>=2)
     {
       *ptangent -= *tangent;
       std::cout << "Difference in Tangent vectors:" << norm(*ptangent)/std::max(nD,nDp) << std::endl;
     }

     std::unique_ptr<AbstractFunctional> fuVPtr(functional->getLinFunctionValue(makePars(p.muTrial.value())));
     std::unique_ptr<AbstractLinearization> paraLinFu(fuVPtr->getLinearization(*trialIterate));
     paraLinFu->evald(*iterate);
     double slopeJ = iterate->applyAsDualTo(*tangent);
     pp.jpl=p.mu.value()*slopeJ;
     double nDP=(*normPlain)(*tangent);
     *ptangent -= *tangent;
     double nDiff=norm(*ptangent);

     pp.slope=nDP;
     pp.curvature=nDiff/p.deltaMu;
     pp.eta=pp.curvature;
     GuardedCovariantNewtonParameters const& cnp(dynamic_cast<GuardedCovariantNewtonParameters const&>(corrector.getParameters()));
     pp.omega=cnp.omega0.value();
     pp.accuracyCorrector=cnp.accuracyReached;
     std::cout << "Norm:" << n/p.deltaMu << " ||x'||_2:" << nDP << " Dmu:" << p.deltaMu << " ||x'||_x:"<< pp.eta << " omega:" << pp.omega << std::endl;
  };

  virtual void computePredictor(int step)
  {
    stp=step;
    if(p.mu==p.muTrial) return;
    if(step==2)
    {
      std::unique_ptr<AbstractFunctional> fuPtr(functional->getParameterLinFunctional(makePars(p.muTrial.value())));
      std::unique_ptr<AbstractLinearization> paraLinearization(fuPtr->getLinearization(*trialIterate));
      solver.solve(*tangent,*paraLinearization);
    }
    *predictor=*iterate;
    predictor->axpy(p.mu-p.muTrial,*tangent);
    DomainVector& tx=Bridge::getImpl<DomainVector>(*iterate);
    DomainVector& px=Bridge::getImpl<DomainVector>(*predictor);
    DomainVector const& cx=Bridge::getImpl<DomainVector>(*tangent);
    std::unique_ptr<AbstractFunctional> fuPtr(functional->getFunctional(makePars(p.mu.value())));
    std::unique_ptr<AbstractLinearization> linPtr(fuPtr->getLinearization(*iterate));
    std::unique_ptr<AbstractLinearization> paraLinearization(fuPtr->getLinearization(*iterate));

    Bridge::Linearization<Implementation> const&  limpl(dynamic_cast<Bridge::Linearization<Implementation> const& >(*linPtr));
    Bridge::Linearization<Implementation> const&  plimpl(dynamic_cast<Bridge::Linearization<Implementation> const& >(*paraLinearization));

    LinearizationAt<Barrier> blin(limpl.getLinImpl().getBarrier().getLinImpl().getFunctional(),tx);
    LinearizationAt<Barrier> plin(plimpl.getLinImpl().getBarrier().getLinImpl().getFunctional(),tx);
    LinearizationAt<Unconstrained> ulin(limpl.getLinImpl().getUnconstrained().getLinImpl().getFunctional(),tx);
    Dune::BlockVector<Dune::FieldVector<typename Unconstrained::Scalar, 1> > temp(*boost::fusion::at_c<Unconstrained::yIdx>(px.data));
    corrTangent(px, tx, blin, plin, ulin, p.muTrial, p.mu);
    for (int i=0; i<temp.size(); ++i)
      if((*boost::fusion::at_c<Barrier::yIdx>(cx.data))[i][0] < 0) (*boost::fusion::at_c<Barrier::yIdx>(px.data))[i][0] = temp[i];
    *trialIterate=*predictor;
    *predictor-=*iterate;
    std::unique_ptr<AbstractFunctional> fuPtr2(functional->getParameterLinFunctional(makePars(p.muTrial.value())));
    std::unique_ptr<AbstractLinearization> paraLinearization2(fuPtr2->getLinearization(*trialIterate));
    solver.solve(*ptangent,*paraLinearization2);
  }

  virtual void initializeCorrector()
  {
    if(p.muTrial < p.muFinal) corrector.setDesiredAccuracy(p.accfinal);
    else
    {
      if(pp.eta.isValid()) corrector.setDesiredAccuracy(1000000*std::max(p.accfinal*0.25,lengthOfPath()*p.relDistanceToPath));
      else corrector.setDesiredAccuracy(sqrt(p.muTrial)*p.relDistanceToPath);
      corrector.setDesiredRelativeAccuracy(p.relDistanceToPath);//std::sqrt(p.muTrial/p.mu));
    }
  };
  virtual void finalizeCorrector() {    nNewton += corrector.stepsPerformed(); pp.newtonSum=nNewton;};

  virtual void finalizeHomotopy()
  {
    std::cout << "----------------------- Final Correction Step ---------------------" << std::endl;
    finalsolver->resetParameters();
    finalsolver->setDesiredAccuracy(p.accfinal);
    finalsolver->setDesiredRelativeAccuracy(1.0);
    std::unique_ptr<AbstractFunctional> fuPtr(functional->getFunctional(makePars(p.muTrial.value())));
    std::cout << "muTrial: " << p.muTrial << std::endl;
    finalsolver->reportOnIteration(true);
    finalsolver->solve(fuPtr.get(), *trialIterate);
    p.correctorTermination=corrector.getParameters().termination;
  }

  virtual void updateIterate() {
    *iterate=*trialIterate;
    p.j=functional->getFunctional(makePars(p.muTrial.value()))->getLinearization(*iterate)->eval();
    jModelL.update(p.muTrial,p.j);
    jModelL.fixUpdate();
    p.jp=jModelL.getValue(p.muTrial);
    std::cout << "Est. Error in Functional:" << p.jp << std::endl;
    pp.timemeasured=overalltime.elapsed();
  };

  virtual void recoverIterate() { *trialIterate=*iterate; }

  virtual double lengthOfPath()
  {
    if(pp.eta.isValid())
      return 2*p.mu*pp.slope;
    else
      return 1e300;
  };

  virtual void logQuantities()
  {
    HomotopyBase::logQuantities();
    pp.logStep();
    printDiagnosis();
  }

  void printDiagnosis()
  {
    std::ofstream eout("eta.log"); pp.eta.print(eout);
    std::ofstream epout("slope.log"); pp.slope.print(epout);
    std::ofstream cpout("curvature.log"); pp.curvature.print(cpout);
    std::ofstream nout("newtonSum.log"); pp.newtonSum.print(nout);
    std::ofstream oout("omega.log"); pp.omega.print(oout);
    std::ofstream sout("sigma.log"); p.sigma.print(sout);
    std::ofstream outmu("mu.log"); p.muTrial.print(outmu);
    std::ofstream outj("j.log"); p.j.print(outj);
    std::ofstream outjp("jp.log"); p.jp.print(outjp);
    std::ofstream outjpl("jplin.log"); pp.jpl.print(outjpl);
    std::ofstream outac("ac.log"); pp.accuracyCorrector.print(outac);
    std::ofstream outtm("time.log"); pp.timemeasured.print(outtm);
  }

private:
  std::unique_ptr<AbstractFunctionSpaceElement> iterate, predictor, tangent, ptangent;

  AbstractNorm const& norm;
  AbstractLinearSolver& solver;
  InteriorPointParametersSqrt pp;
  JModelLin jModelL;
  int stp;
  AbstractNorm const* normPlain;
  NewtonsMethod* finalsolver;
  int nNewton;
  boost::timer::cpu_timer overalltime;
};

/// Policy class that performs a pointwise damped Newton step
// template<class Functional, class DomainElement,class Implementation=Bridge::IPLinearization<Functional, DomainElement> >
// class StepPolicyPointwiseDamping : public StepPolicyPlainNewton
// {
// public:
//   typedef typename Functional::OptimalityFunctional Unconstrained;
//   typedef Functional Combined;

//   virtual void setLinearization(AbstractLinearization& lin, AbstractLinearSolver& solver)
//   {
//     linPtr= &lin;
//     linearSolver=&solver;
//     mu=dynamic_cast<Bridge::Linearization<Implementation>& >(lin).getLinImpl().getMu();
//   }

//   virtual void getTrialIterate(
//     AbstractFunctionSpaceElement& trialIterate,
//     AbstractFunctionSpaceElement const& correction,
//     AbstractFunctionSpaceElement const& iterate,
//     double damping)
//   {
//     trialIterate = iterate;
//     trialIterate.axpy(damping,correction);
//     dcorrection = correction.clone();
//     *dcorrection*=damping;
//     correctIterate(trialIterate, *dcorrection, *linPtr, iterate, mu);
//   }
// private:

//   std::unique_ptr<AbstractFunctionSpaceElement> dcorrection;

// /* Makes a pointwise damping correction. The following parameters are used:
//  * - trialIterate: iterate after (simplified) Newton step -> iterate after pointwise damping
//  * - correction:   (simplified) Newton correction
//  * - lin       :   point of linearization/ iterate before Newton step
//  * - siterate  :   point, where rhs is evaluated, iterate after Newton step
//  *
//  */
//   void correctIterate(AbstractFunctionSpaceElement & trialIterate,
//                       AbstractFunctionSpaceElement const& correction,
//                       AbstractLinearization const& lin,
//                       AbstractFunctionSpaceElement const& siterate,
//                       double mu)
//   {
//     using namespace boost::fusion;
//     DomainElement const& x=Bridge::getImpl<DomainElement>(lin.getOrigin());
//     DomainElement const& sx=Bridge::getImpl<DomainElement>(siterate);
//     DomainElement& tx=Bridge::getImpl<DomainElement>(trialIterate);

//     Bridge::Linearization<Implementation> const&  limpl(dynamic_cast<Bridge::Linearization<Implementation> const& >(lin));

//     LinearizationAt<Unconstrained> ulin(limpl.getLinImpl().getUnconstrained().getLinImpl().getFunctional(),tx);
//     QuadraticModel<LinearizationAt<Combined>,true> blin(linearization(limpl.getLinImpl().getBarrier().getLinImpl().getFunctional(),sx),
//                                                        linearization(limpl.getLinImpl().getBarrier().getLinImpl().getFunctional(),x),
//                                                        Bridge::getImpl<DomainElement>(correction));
//     PointwiseCorrection<FirstOrderEquation> pc;

//     pc.correct(tx, sx, blin, ulin, mu);
//   }
//   double mu;

// };

#endif


/*
 * @file test.cpp
 * @author Takuya HAYASHI <t-hayashi@nict.go.jp>
 */

#include "Rq.hpp"

using namespace fntt;

#include <NTL/ZZ_pX.h>
template<size_t N, size_t qNum, enum NTTSelector NTT, enum ImplSelector Impl>
void MulNTL(Rq<N, qNum, NTT, Impl> &c, const Rq<N, qNum, NTT, Impl> &a, const Rq<N, qNum, NTT, Impl> &b) {
  for(size_t qIdx = 0; qIdx < qNum; ++qIdx) {
    NTL::ZZ_p::init(NTL::conv<NTL::ZZ>((unsigned long)Rq<N, qNum, NTT, Impl>::param_.p[qIdx]));
    NTL::ZZ_pX f;
    f.SetLength(N + 1);
    f[N] = f[0] = 1;
    NTL::ZZ_pXModulus F(f);

    NTL::ZZ_pX a_, b_, c_;
    a_.SetLength(N);
    b_.SetLength(N);
    c_.SetLength(N);

    for(size_t i = 0; i < N; ++i) a_[i] = a(qIdx, i);
    for(size_t i = 0; i < N; ++i) b_[i] = b(qIdx, i);

    NTL::MulMod(c_, a_, b_, F);

    for(size_t i = 0; i < N; ++i) c(qIdx, i) = NTL::conv<long>(c_[i]);
  }
}

template <size_t N, size_t qNum, enum NTTSelector NTT, enum ImplSelector Impl>
bool testMulMax() {
  using R = Rq<N, qNum, NTT, Impl>;
  R a, b, c, c_;
  for(size_t qIdx = 0; qIdx < qNum; ++qIdx) {
    for(size_t i = 0; i < N; ++i) {
      a(qIdx, i) = R::param_.p[qIdx] - 1UL;
      b(qIdx, i) = R::param_.p[qIdx] - 1UL;
    }
  }

  MulNTL<N, qNum, NTT, Impl>(c, a, b);
  R::mul(c_, a, b);

  return c != c_;
}

template<size_t N, size_t qNum, enum NTTSelector NTT, enum ImplSelector Impl>
void TestMul(const size_t niter = 128) {
  using R = Rq<N, qNum, NTT, Impl>;
  R a, b, c, c_;

  printf("mul(%lu %lu %d %d): ", N, qNum, NTT, Impl);

  if(testMulMax<N, qNum, NTT, Impl>()) {
    printf("x max\n");
    return;
  }

  for(size_t i = 0; i < niter; ++i) {
    a.rand();
    b.rand();

    MulNTL<N, qNum, NTT, Impl>(c, a, b);
    R::mul(c_, a, b);

    if(c != c_) {
      printf("x rand\n");
      return;
    }
  }
  printf("o\n");
}

static constexpr size_t qNum = 2;

int main() {

  TestMul<4096, qNum, LN16, Generic>();
  TestMul<8192, qNum, LN16, Generic>();
  TestMul<16384, qNum, LN16, Generic>();
  TestMul<32768, qNum, LN16, Generic>();

  TestMul<4096, qNum, LN16, x86_64>();
  TestMul<8192, qNum, LN16, x86_64>();
  TestMul<16384, qNum, LN16, x86_64>();
  TestMul<32768, qNum, LN16, x86_64>();

  TestMul<4096, qNum, S17, Generic>();
  TestMul<8192, qNum, S17, Generic>();
  TestMul<16384, qNum, S17, Generic>();
  TestMul<32768, qNum, S17, Generic>();

  TestMul<4096, qNum, S17, x86_64>();
  TestMul<8192, qNum, S17, x86_64>();
  TestMul<16384, qNum, S17, x86_64>();
  TestMul<32768, qNum, S17, x86_64>();

  TestMul<4096, qNum, FNTT, Generic>();
  TestMul<8192, qNum, FNTT, Generic>();
  TestMul<16384, qNum, FNTT, Generic>();
  TestMul<32768, qNum, FNTT, Generic>();

  TestMul<4096, qNum, FNTT, x86_64>();
  TestMul<8192, qNum, FNTT, x86_64>();
  TestMul<16384, qNum, FNTT, x86_64>();
  TestMul<32768, qNum, FNTT, x86_64>();

  TestMul<4096, qNum, FNTT, SSE>();
  TestMul<8192, qNum, FNTT, SSE>();
  TestMul<16384, qNum, FNTT, SSE>();
  TestMul<32768, qNum, FNTT, SSE>();

  TestMul<4096, qNum, FNTT, AVX2>();
  TestMul<8192, qNum, FNTT, AVX2>();
  TestMul<16384, qNum, FNTT, AVX2>();
  TestMul<32768, qNum, FNTT, AVX2>();

  return 0;
}


#ifndef HW4_OBJECT_HPP
#define HW4_OBJECT_HPP

#include <array>
#include <memory>

#include <boost/filesystem.hpp>
#include <boost/optional.hpp>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>

#include "bvh.hpp"
#include "material.hpp"
#include "ray.hpp"

namespace hw4 {
    constexpr float tau = 6.2831853071f;

    class Intersection {
        glm::vec3 m_point;
        glm::vec3 m_normal;
        glm::vec2 m_texcoord;

        Material* m_material;

        float m_distance;
    public:
        Intersection() : m_distance(0) {}
        Intersection(
            glm::vec3 point,
            glm::vec3 normal,
            glm::vec2 texcoord,
            Material* material,
            float distance
        )
            : m_point(point), m_normal(normal), m_texcoord(texcoord),
              m_material(std::move(material)), m_distance(distance) {}

        const glm::vec3& point() const { return this->m_point; }
        const glm::vec3& normal() const { return this->m_normal; }
        const glm::vec2& texcoord() const { return this->m_texcoord; }
        float distance() const { return this->m_distance; }

        PointMaterial material() const { return this->m_material->at_point(this->m_texcoord); }
        Intersection& material(Material* material) {
            this->m_material = material;
            return *this;
        }

        Intersection transform(const glm::mat4& transform, float dist_mult) const {
            return Intersection(
                glm::vec3(transform * glm::vec4(this->m_point, 1)),
                // Since we only allow orthogonal transformations and translations, we can just use
                // the transform directly.
                glm::normalize(glm::mat3(transform) * this->m_normal),
                this->m_texcoord,
                this->m_material,
                this->m_distance * dist_mult
            );
        }
    };

    class Object {
        glm::mat4 m_transform;
        glm::mat4 m_inv_transform;

        BoundingBox m_obb;
        BoundingBox m_aabb;

        std::shared_ptr<Material> m_material;
    public:
        Object(
            BoundingBox obb,
            const glm::mat4& transform,
            const std::shared_ptr<Material>& material
        )
            : m_transform(transform), m_inv_transform(glm::inverse(transform)), m_obb(obb),
              m_aabb(transform * obb), m_material(material) {}
        virtual ~Object() = default;

        const glm::mat4& transform() const { return this->m_transform; }
        Object& transform(const glm::mat4& transform) {
            this->m_transform = transform;
            this->m_inv_transform = glm::inverse(this->m_transform);
            this->m_aabb = transform * this->m_obb;

            return *this;
        }

        const glm::mat4& inv_transform() const { return this->m_inv_transform; }

        const BoundingBox& obb() const { return this->m_obb; }
        const BoundingBox& aabb() const { return this->m_aabb; }

        const std::shared_ptr<Material>& material() const { return this->m_material; }

        virtual boost::optional<Intersection> find_intersection(const Ray& r) const = 0;
    };

    class SphereObject : public Object {
    public:
        SphereObject(
            float radius,
            glm::vec3 center,
            const std::shared_ptr<Material>& material
        );

        virtual boost::optional<Intersection> find_intersection(const Ray& r) const;
    };

    struct Vertex {
        glm::vec3 pos;
        glm::vec3 normal;
        glm::vec2 texcoord;
    };

    struct Triangle {
        unsigned int a;
        unsigned int b;
        unsigned int c;
    };

    inline BoundingBox calc_bounding_box(const std::vector<Vertex>& vertices) {
        if (vertices.empty()) return BoundingBox();

        glm::vec3 min(vertices[0].pos);
        glm::vec3 max(vertices[0].pos);

        for (const auto& v : vertices) {
            if (v.pos.x < min.x) min.x = v.pos.x;
            if (v.pos.x > max.x) max.x = v.pos.x;

            if (v.pos.y < min.y) min.y = v.pos.y;
            if (v.pos.y > max.y) max.y = v.pos.y;

            if (v.pos.z < min.z) min.z = v.pos.z;
            if (v.pos.z > max.z) max.z = v.pos.z;
        }

        return BoundingBox(min, max);
    }

    class TriMesh {
        std::vector<Vertex> m_vertices;
        std::vector<Triangle> m_triangles;
        BVH<Triangle> m_bvh;
        BoundingBox m_obb;

        BoundingBox calc_triangle_bounding_box(const Triangle& t) {
            const auto& a = this->m_vertices[t.a].pos;
            const auto& b = this->m_vertices[t.b].pos;
            const auto& c = this->m_vertices[t.c].pos;

            return BoundingBox(
                glm::vec3(
                    std::min(a.x, std::min(b.x, c.x)),
                    std::min(a.y, std::min(b.y, c.y)),
                    std::min(a.z, std::min(b.z, c.z))
                ),
                glm::vec3(
                    std::max(a.x, std::max(b.x, c.x)),
                    std::max(a.y, std::max(b.y, c.y)),
                    std::max(a.z, std::max(b.z, c.z))
                )
            );
        }
    public:
        TriMesh(std::vector<Vertex> vertices, std::vector<Triangle> triangles)
            : m_vertices(std::move(vertices)), m_triangles(std::move(triangles)),
              m_obb(calc_bounding_box(this->m_vertices)) {}

        TriMesh& regen_bvh(size_t delta) {
            this->m_bvh = BVH<Triangle>::construct(
                ([this]() {
                    std::vector<Triangle*> ts;

                    for (auto& t : this->m_triangles) {
                        ts.push_back(&t);
                    }

                    return ts;
                })(),
                delta,
                [this](const auto& t) { return this->calc_triangle_bounding_box(t); }
            );

            return *this;
        }

        const BoundingBox& obb() const { return this->m_obb; }

        const std::vector<Vertex>& vertices() const { return this->m_vertices; }
        const std::vector<Triangle>& triangles() const { return this->m_triangles; }
        const BVH<Triangle>& bvh() const { return this->m_bvh; }

        boost::optional<Intersection> find_intersection(const Ray& r, const Triangle& t) const;
        boost::optional<Intersection> find_intersection(const Ray& r) const;

        static std::shared_ptr<TriMesh> load_mesh(boost::filesystem::path path);
    };

    class TriMeshObject : public Object {
        std::shared_ptr<TriMesh> m_mesh;
    public:
        TriMeshObject(
            std::shared_ptr<TriMesh> mesh,
            const glm::mat4& transform,
            const std::shared_ptr<Material>& material
        ) : Object(mesh->obb(), transform, material), m_mesh(std::move(mesh)) {}

        const std::shared_ptr<TriMesh>& mesh() const { return this->m_mesh; }

        virtual boost::optional<Intersection> find_intersection(const Ray& r) const;
    };

    inline glm::mat4 apply_orientation(const glm::mat4& transform, const glm::vec3& rot) {
        return glm::rotate(
            glm::rotate(
                glm::rotate(transform, rot.x, glm::vec3(0, 1, 0)),
                rot.y,
                glm::vec3(1, 0, 0)
            ),
            rot.z,
            glm::vec3(0, 0, 1)
        );
    }
}

#endif


#ifndef STAN_MATH_PRIM_SCAL_PROB_CHI_SQUARE_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_CHI_SQUARE_RNG_HPP

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/gamma_p.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_gamma.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
  namespace math {

    /**
     * Return a pseudorandom chi squared variate with the nu degrees of freedom
     * using the specified random number generator.
     *
     * @tparam RNG class of random number generator
     * @param nu positive degrees of freedom parameter
     * @param rng random number generator
     * @return chi squared random variate
     * @throw std::domain_error if nu is nonpositive
     */
    template <class RNG>
    inline double
    chi_square_rng(double nu,
                   RNG& rng) {
      using boost::variate_generator;
      using boost::random::chi_squared_distribution;

      static const char* function("chi_square_rng");

      check_positive_finite(function, "Degrees of freedom parameter", nu);

      variate_generator<RNG&, chi_squared_distribution<> >
        chi_square_rng(rng, chi_squared_distribution<>(nu));
      return chi_square_rng();
    }

  }
}
#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include <sensor_msgs/Imu.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Pose.h>

#define PI 3.14159265

using namespace std;
using namespace Eigen;
Matrix<float, 3, 3> R;
Matrix<float, 3, 3> R_inv;
Matrix<float, 3, 3> pos;
geometry_msgs::PoseStamped pos_sp;
Quaternionf quat;

float x = 0, y = 0, z=0;
float x_glob = 0, y_glob = 0, z_glob=0;
float pos_x_prev = 0, pos_y_prev = 0;
int aruco_detected_flag=0;
int pose_detected_flag=0;
float sp_thresh=0.1;
float z_dist=0.1;

void imuCallback(const sensor_msgs::Imu::ConstPtr &msg)
{
    tf::Quaternion q1(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
    //tf::Matrix3x3 m(q1);
    //double r, p,imu_yaw;
    //m.getRPY(r, p, imu_yaw);
    //cout<<r*180/3.14<<"   "<<p*180/3.14<<"   "<<imu_yaw*180/3.14<<endl<<endl;

  quat=Eigen::Quaternionf(q1.w(),q1.x(),q1.y(),q1.z());
  R=quat.toRotationMatrix();

}

void odomcb(const geometry_msgs::Pose::ConstPtr &msg)
{
    x_glob = msg->position.x;
    y_glob = msg->position.y;
    z_glob = msg->position.z;
    pose_detected_flag = 1;
    pos_sp.pose.orientation=msg->orientation;

}

void arucocb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    y = -msg->pose.position.x;
    x = -msg->pose.position.y;
    z = -msg->pose.position.z;
    aruco_detected_flag = 1;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "object_pos");
    ros::NodeHandle nh;
    ros::Subscriber imu_sub = nh.subscribe("imu/data", 10, imuCallback);
    ros::Subscriber aruco_sub = nh.subscribe("aruco_single/pose", 10, arucocb);
    ros::Subscriber odom_sub = nh.subscribe("odometry", 10, odomcb);

    ros::Publisher pos_sp_pub = nh.advertise<geometry_msgs::PoseStamped>("object/pose", 10);

    ros::Rate loop_rate(20);

    while (ros::ok())
    {

        pos_sp.header.stamp = ros::Time::now();

        if (aruco_detected_flag == 1 && pose_detected_flag == 1)
        {
            pos(0,0)=x;
            pos(1,0)=y;
            pos(2,0)=z;

            R_inv = R.inverse();
            pos = R*pos ;

            //cout<<x<<"--"<<y<<"****"<<pos(0,0)<<"--"<<pos(1,0)<<endl;
	  /*  if(pos(0,0)<0.1)
		pos(0,0)=pos_x_prev;
	    if(pos(1,0)<0.1)
		pos(1,0)=pos_y_prev;*/
            pos_sp.pose.position.x=pos(0,0)+x_glob;
            pos_sp.pose.position.y=pos(1,0)+y_glob;

            pos_sp.pose.position.z = z_glob;
            pos_sp_pub.publish(pos_sp);

            aruco_detected_flag = 0;
	    pose_detected_flag = 0;

		pos_y_prev=pos(1,0);
		pos_x_prev=pos(0,0);

        }

       // pos_sp_pub.publish(pos_sp);
        ros::spinOnce();
        loop_rate.sleep();

    }

    return 0;
}


//==================================================================================================
/**
  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_ARCH_PPC_VMX_SIMD_FUNCTION_REC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_PPC_VMX_SIMD_FUNCTION_REC_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/constant/inf.hpp>

#if !defined( BOOST_SIMD_NO_INFINITIES )
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/is_inf.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename A0)
                          , bs::vmx_
                          , bs::pack_< bd::floating_<A0>, bs::vmx_>
                          )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      A0 estimate = fast_(rec)(a0);

      // fix rec(+/-0)
      estimate = if_else( is_eqz(a0)
                        , copysign(Inf<A0>(),a0)
                        , estimate
                        );

      // fix rec(+/-inf)
      #if !defined( BOOST_SIMD_NO_INFINITIES )
      estimate = if_else( is_inf(a0)
                        , copysign(Zero<A0>(),a0)
                        , estimate
                        );
      #endif

      return estimate;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename A0)
                          , bs::vmx_
                          , bs::raw_tag
                          , bs::pack_< bd::single_<A0>, bs::vmx_>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(bs::raw_tag const&, const A0& a0) const BOOST_NOEXCEPT
    {
      return vec_re( a0.storage() );
    }
  };
} } }

#endif


#include <iostream>
#include <fstream>
#include <cmath>
#include <armadillo>
#include <gtkmm-3.0/gtkmm.h>

// Compílelo así:
//g++ py1.cpp -larmadillo -llapack -lblas `pkg-config gtkmm-3.0 --cflags --libs`
//Hola hola
class Sustancia
{
    public:
        std::string nombre;
        int indiceS;
        double v25; //volumen a 25°C en cm³/mol
        double vb; // volumen en el punto de ebullición en cm³/mol
        double delta25; // delta25, ignoro que es eso
        double T_eb; // temperatura de ebullición a 1 bar
        double A;
        double B;
        double C; //constantes de Antoine en log base 10 y C en Kelvin
        arma::mat parametrosInteraccion;
};

//Bueno... ya sabes que puedes hacer funciones que devuelvan arma::mat directamente sin void.
//Sin embargo, se hizo el programa a punta de pointers... grande...
void obtenerSustancia(int indice, arma::mat Datos, arma::mat Interaccion, Sustancia* dirSustancia);
void ecuacionesWilson(arma::mat vars, double x, double P, Sustancia sus1, Sustancia sus2, arma::mat* dirResp);
void jacobiano(arma::mat vars, double x, double P, Sustancia comp1, Sustancia comp2, double h, arma::mat* dirJac);
void equilibrio(double x, double P, Sustancia comp1, Sustancia comp2, arma::mat vecIni, arma::mat* dirResp);

int main(int argc, char *argv[])
{
    arma::mat losDatos;
    arma::mat Interacciones;
    losDatos.load("datosSustancias.csv");
    Interacciones.load("ParametrosInteraccion.csv");
    Sustancia *dirComp1 = new Sustancia;
    Sustancia *dirComp2 = new Sustancia;
    obtenerSustancia(25,losDatos,Interacciones,dirComp1);
    obtenerSustancia(30,losDatos,Interacciones,dirComp2);
    Sustancia laSustancia1=*dirComp1;
    Sustancia laSustancia2=*dirComp2;
    arma::mat variablesIni = arma::mat{0.4,353}.t();
    arma::mat *dirRespuesta = new arma::mat;
    equilibrio(0.3,101.325,laSustancia1,laSustancia2,variablesIni,dirRespuesta);
    std::cout<<"La respuesta es: \n"<<(*dirRespuesta)<<"\n";
    delete dirComp1;
    delete dirComp2;
    delete dirRespuesta;
    return 0;
}

void obtenerSustancia(int indice, arma::mat Datos, arma::mat Interaccion, Sustancia* dirSustancia)
{
    /*
    Esta función devuelve una sustancia de la lista de los datos de las sustancias y de la lista de parámetros de interacción,
    dado un número entero. Se devolverá la sustancia de la [número entero de entrada "índice"] fila del archivo de los datos de las sustancias
    */
    Sustancia compuesto;
    compuesto.v25=Datos(indice,1);
    compuesto.vb=Datos(indice,2);
    compuesto.delta25=Datos(indice,3);
    compuesto.T_eb=Datos(indice,4);
    compuesto.A=Datos(indice,5);
    compuesto.B=Datos(indice,6);
    compuesto.C=Datos(indice,7);
    compuesto.parametrosInteraccion=Interaccion.row(indice);
    compuesto.indiceS=indice;
    *dirSustancia=compuesto;
}

/*
Esta función determina el equilibrio de un sistema binario dada una composición líquida y la presión. Es decir, dada la composición líquida, detrmina
la composición de valor y la temperatura.
*/
void equilibrio(double x, double P, Sustancia comp1, Sustancia comp2, arma::mat vecIni, arma::mat* dirResp)
{
    int iter, maxIter;
    double error, tol, h;
    arma::mat vnuevo;
    arma::mat v0;
    iter=0;
    maxIter=100;
    error=1000;
    tol=1e-6;
    h=1e-7;
    v0=vecIni;
    arma::mat *dirfv0 = new arma::mat;
    arma::mat *dirJacobiano = new arma::mat;
    while (error>tol && iter<=maxIter)
    {
        jacobiano(v0,x,P,comp1,comp2,h,dirJacobiano);
        ecuacionesWilson(v0,x,P,comp1,comp2,dirfv0);
        vnuevo=v0-arma::solve((*dirJacobiano),(*dirfv0));
        iter++;
        error=std::abs(arma::norm(vnuevo,1)-arma::norm(v0,1))/arma::norm(v0,1);
        v0=vnuevo;
    }
    if (iter>maxIter)
    {
        std::cout<<"OJO! Máximas iteraciones alcanzadas \n";
    }
    delete dirJacobiano;
    delete dirfv0;
    (*dirResp)=vnuevo;
}

void jacobiano(arma::mat vars, double x, double P, Sustancia comp1, Sustancia comp2, double h, arma::mat* dirJac)
{
    /*
    Esta función devuelve el jacobiano de la función ecuaciones Wilson en un punto
    */
    int nVars=vars.n_elem;
    arma::mat vec_h, vec_hmas, derivada;
    arma::mat Jac;
    Jac=arma::zeros<arma::mat>(nVars,nVars);
    arma::mat *dirEcu0 = new arma::mat;
    arma::mat *dirEcuh = new arma::mat;
    for (int i=0; i<nVars;i++)
    {
        vec_h.zeros(2); //Por defecto zeros y ones hace vectores columna
        vec_h(i)=h;
        vec_hmas=vars+vec_h;
        ecuacionesWilson(vars,x,P,comp1,comp2,dirEcu0);
        ecuacionesWilson(vec_hmas,x,P,comp1,comp2,dirEcuh);
        derivada=(1/h)*((*dirEcuh)-(*dirEcu0));
        Jac.col(i)=derivada;
    }
    delete dirEcu0;
    delete dirEcuh;
    (*dirJac)=Jac;
}
/*
Esta función iguala las ecuaciones de equilibrio a cero:
dirResp(0)=y1*P-Gamma1*x1*P1sat;
dirResp(1)=y2*P-Gamma2*x2*P2sat;
El modelo usado es el modelo de Wilson
*/
void ecuacionesWilson(arma::mat vars, double x, double P, Sustancia sus1, Sustancia sus2, arma::mat* dirResp)
{
    double y1, y2, x1, x2, T;
    //parámetros
    double v[2], delta[2], beta[2], lambdas[2][2], parInter[2], Atlantis[2], logGamma[2], Gamma[2], Psat[2], resp[2];
    int z=2;
    double R=8.314;
    // este guarda la respuesta
    y1=vars(0);
    T=vars(1); // Esta debe entrar a esta función en Kelvin
    x1=x;
    y2=1-y1;
    x2=1-x1;
    beta[0]=(sus1.vb-sus1.v25)/(sus1.T_eb-25);
    beta[1]=(sus2.vb-sus2.v25)/(sus2.T_eb-25);
    v[0]=sus1.v25+beta[0]*((T-273.15)-25); //En el cálculo de los volúmenes la temperatura está en °C
    v[1]=sus2.v25+beta[1]*((T-273.15)-25); //En el cálculo de los volúmenes la temperatura está en °C
    delta[0]=(sus1.v25/v[0])*sus1.delta25;
    delta[1]=(sus2.v25/v[1])*sus2.delta25;
    parInter[0]=sus1.parametrosInteraccion(sus2.indiceS);
    parInter[1]=sus2.parametrosInteraccion(sus1.indiceS);
    lambdas[0][1]=-(1-parInter[0])*(2/z+0.0)*pow(v[0]*v[1],0.5)*delta[0]*delta[1];
    lambdas[1][0]=-(1-parInter[1])*(2/z+0.0)*pow(v[1]*v[0],0.5)*delta[1]*delta[0];
    lambdas[0][0]=-(2/z+0.0)*v[0]*pow(delta[0],2);
    lambdas[1][1]=-(2/z+0.0)*v[1]*pow(delta[1],2);
    Atlantis[0]=(v[1]/v[0])*exp(-(lambdas[0][1]-lambdas[0][0])/(R*T));
    Atlantis[1]=(v[0]/v[1])*exp(-(lambdas[1][0]-lambdas[1][1])/(R*T));
    logGamma[0]=-log(x1+Atlantis[0]*x2)+x2*((Atlantis[0]/(x1+Atlantis[0]*x2))-(Atlantis[1]/(Atlantis[1]*x1+x2)));
    logGamma[1]=-log(Atlantis[1]*x1+x2)-x1*((Atlantis[0]/(x1+Atlantis[0]*x2))-(Atlantis[1]/(Atlantis[1]*x1+x2)));
    Gamma[0]=exp(logGamma[0]);
    Gamma[1]=exp(logGamma[1]);
    Psat[0]=pow(10,sus1.A-(sus1.B/(T-sus1.C)));
    Psat[1]=pow(10,sus2.A-(sus2.B/(T-sus2.C)));
    resp[0]=y1*P-Gamma[0]*x1*Psat[0];
    resp[1]=y2*P-Gamma[1]*x2*Psat[1];
    (*dirResp)=arma::mat{resp[0],resp[1]}.t();
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_LGAMMA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LGAMMA_HPP

/*
 * The lgamma implementation in stan-math is based on either the
 * reentrant safe lgamma_r implementation from C or the
 * boost::math::lgamma implementation. The reentrant safe lgamma_r
 * implementation is preferred as it is faster when compared to the
 * boost version. However, the reentrant safe lgamma_r C function is
 * not available with MinGW compilers used on Windows. Therefore, the
 * boost version is used on Windows with MinGW compilers as fall
 * back. For details on the speed evaluations, please refer to
 * https://github.com/stan-dev/math/pull/1255 .
 */
#if !__MINGW32__
// _REENTRANT must be defined during compilation to ensure that cmath
// exports the reentrant safe lgamma_r version.
#if !_REENTRANT
#error \
    "stan-math requires _REENTRANT being defined during compilation" \
    "to make lgamma_r available."
#endif
#include <cmath>
#else
// MinGW compilers on Windows do not provide the reentrant lgamma_r
// such that we fall back to boost whenever we are on MinGW.
#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <limits>
#endif

namespace stan {
namespace math {

/**
 * Return the natural logarithm of the gamma function applied to
 * the specified argument.
 *
   \f[
   \mbox{lgamma}(x) =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \ln\Gamma(x) & \mbox{if } x\not\in \{\dots, -3, -2, -1, 0\}\\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{lgamma}(x)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \Psi(x) & \mbox{if } x\not\in \{\dots, -3, -2, -1, 0\}\\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
\f]
*
* @param x argument
* @return natural logarithm of the gamma function applied to
* argument
*/
inline double lgamma(double x) {
#if !__MINGW32__
  int sign = 1;
  return ::lgamma_r(x, &sign);
#else
  if (unlikely(x == 0.0))
    return std::numeric_limits<double>::infinity();
  return boost::math::lgamma(x, boost_policy_t());
#endif
}

/**
 * Return the natural logarithm of the gamma function applied
 * to the specified argument.
 *
 * @param x argument
 * @return natural logarithm of the gamma function applied to
 * argument
 */
inline double lgamma(int x) {
#if !__MINGW32__
  int sign = 1;
  return ::lgamma_r(x, &sign);
#else
  if (unlikely(x == 0.0))
    return std::numeric_limits<double>::infinity();
  return boost::math::lgamma(x, boost_policy_t());
#endif
}

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


// MIT License
//
// Copyright (c) 2019 Ivan Eichhardt
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// Please contact the author of this library if you have any questions.
// Author: Ivan Eichhardt (ivan.eichhardt@sztaki.mta.hu)
#pragma once

#include <Eigen/Dense>

namespace higher_order {

  // an alias to indexing views
  using ViewID = uint32_t;

  // 2D vector with double elements
  using Vec2d = Eigen::Matrix<double, 2, 1>;
  // 3D vector with double elements
  using Vec3d = Eigen::Matrix<double, 3, 1>;
  // 2-by-2 matrix with double elements
  using Mat2d = Eigen::Matrix<double, 2, 2>;
  // 2-by-2 matrix with double elements
  using Mat3d = Eigen::Matrix<double, 3, 3>;
  // 2-by-2 matrix with double elements
  using Mat23d = Eigen::Matrix<double, 2, 3>;
  // 3-by-2 matrix with double elements
  using Mat32d = Eigen::Matrix<double, 3, 2>;
  // N-by-2 dynamic matrix with double elements
  using MatX2d = Eigen::Matrix<double, Eigen::Dynamic, 2>;

  /*! \class EpipolarConstraintOnLAFs
   * A struct that holds the vector-coefficients of a first-order epipolar constraint, and indices to the corresponding pair of view.
   */
  struct EpipolarConstraintOnLAFs {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    /*! The first vector coefficient of the first-order epipolar constraint */
    Vec2d a;
    /*! The second vector coefficient of the first-order epipolar constraint */
    Vec2d b;

    /*! Indices to a pair of views */
    std::pair < ViewID, ViewID > view_pair;
  };

  /*! \class ObservedLAF
   * A struct that encapsulates the linear transformation part of a Local Affine Frame, and and index to the corresponding view.
   */
  struct ObservedLAF {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    /*! The linear transformation part of a Local Affine Frame */
    Mat2d M;
    /*! The view-index of the corresponding Local Affine Frame */
    ViewID viewID;
  };

  // A vector containing first-order epipolar constraints (EpipolarConstraintOnLAFs)
  using EpipolarConstraints = std::vector<EpipolarConstraintOnLAFs, Eigen::aligned_allocator<EpipolarConstraintOnLAFs>>;

  // A vector constaining the linear transformation parts of Local Affine Frames (ObservedLAF)
  using ObservedLAFs = std::vector<ObservedLAF, Eigen::aligned_allocator<ObservedLAF>>;
}


#include <exception>
#include <fstream>
#include <sstream>
#include <iostream>
#include <iterator>
#include <string>
#include <array>
#include <range/v3/all.hpp>
#include <cstdint>
#include <boost/intrusive/slist.hpp>
#include <boost/intrusive/list.hpp>
#include <initializer_list>
using gint = std::int64_t;
namespace intrusive = boost::intrusive;
namespace views = ranges::views;
template<class Rng>
int finddst(Rng rng, int n, int N)
{
  auto b = rng.begin();
  for (int i = 0;; ++i) {
    int next = (n - i - 1) + (n - i - 1 < 1) * N;
    if (ranges::find_if(rng, [next](const auto &mc) { return mc.int_ == next; }) == rng.end()) {
      return next;
    }
  }
  return N;
}
class MyClass : public boost::intrusive::list_base_hook<>
{
public:
  int int_;
  friend std::ostream &operator<<(std::ostream &os, const MyClass &mc)
  {
    os << mc.int_;
    return os;
  }
  auto operator<=>(int r) const
  {
    return int_ <=> r;
  }

  auto operator<=>(MyClass r) const
  {
    return int_ <=> r.int_;
  }
  MyClass(int i) : int_(i) {}
  MyClass() {}
};
struct CrabCup
{
public:
  typedef intrusive::list<MyClass> BaseList;

private:
  std::vector<MyClass> values;
  BaseList baselist;

public:
  CrabCup(std::initializer_list<int> l, int n) : values(n)
  {
    auto idsbegin = views::ints | views::drop(1) | views::take(l.size()) | views::common;

    int i = 0;
    for (auto t : idsbegin) {
      values[i].int_ = t;
      ++i;
    }
    for (auto t : l) {
      baselist.push_back(values[t - 1]);
    }

    auto indexs = views::ints | views::drop(l.size() + 1) | views::take(values.size() - l.size()) | views::common;
    for (auto t : indexs) {
      values[i].int_ = t;
      baselist.push_back(values[i]);
      ++i;
    }
  }

  friend std::ostream &operator<<(std::ostream &os, const CrabCup &crabcup)
  {
    std::ostream_iterator<int> oit(std::cout, ",");
    ranges::transform(crabcup.baselist, oit, [](auto const &mc) { return mc.int_; });
    return os;
  }
  void round()
  {
    auto &n = baselist.front();
    auto rng = baselist | views::drop(1) | views::take(3);
    auto id = finddst(rng, n.int_, values.size());
    auto iter = baselist.iterator_to(values[id - 1]);
    ++iter;
    auto b = std::next(baselist.begin(), 1);
    auto e = std::next(b, 3);
    baselist.splice(iter, baselist, b, e);
    baselist.pop_front();
    baselist.push_back(n);
  }
  gint findone()
  {
    auto iter = baselist.iterator_to(values[1 - 1]);
    auto n1i = ++iter;
    auto n2i = ++iter;
    return gint(n1i->int_) * (n2i->int_);
  }
};

int main()
{
  CrabCup c({ 4, 6, 7, 5, 2, 8, 1, 9, 3 }, 1000000);
  for (auto i : views::ints | views::take(10000000) | views::common) {
    c.round();
  }
  std::cout << c.findone() << std::endl;
  return 0;
}


#pragma once

/* ROS specific interface */
#include "geometry_msgs/TwistStamped.h"
#include <unsupported/Eigen/MatrixFunctions>
#include "nav_msgs/Odometry.h"
#include "utils/measurement.h"
#include <stdint.h>
#include <vector>
#include <string>
#include <memory>
#include <Eigen/Geometry>
#include <Eigen/Dense>

namespace husky_inekf {

    class CameraOdomMeasurement : public Measurement {
        public:
            // Construct Encoder measurement
            CameraOdomMeasurement() {
                type_ = CAMERA_ODOM;
                translation_ = Eigen::Vector3d::Zero();
                rotation_ = Eigen::Matrix3d::Identity();
                transformation_ = Eigen::Matrix4d::Identity();
            }

            CameraOdomMeasurement(const nav_msgs::Odometry& camera_odom_msg){
                type_ = CAMERA_ODOM;

                translation_ = Eigen::Vector3d::Zero();

                rotation_ = Eigen::Matrix3d::Identity();

                transformation_ = Eigen::Matrix4d::Identity();

                setTranslation(camera_odom_msg.pose.pose.position); // [x, y, z]
                setRotation(camera_odom_msg.pose.pose.orientation); // [x, y, z]
                setHeader(camera_odom_msg.header);

                setTransformation();
            }

            void setTranslation(const geometry_msgs::Point& position_in) {

                translation_(0) = position_in.x;
                translation_(1) = position_in.y;
                translation_(2) = position_in.z;

            }

            void setRotation(const geometry_msgs::Quaternion& orientation_in) {
                Eigen::Quaternion<double> orientation_quat(orientation_in.w,
                                                            orientation_in.x,
                                                            orientation_in.y,
                                                            orientation_in.z);
                rotation_ = orientation_quat.toRotationMatrix();
            }

            void setTransformation() {
                transformation_.block<3,3>(0,0) = rotation_;
                transformation_.block<3,1>(0,3) = translation_;
            }

            inline const Eigen::Matrix4d&  getTransformation() const {

                return transformation_;
            }

        private:

            Eigen::Vector3d translation_;
            Eigen::Matrix3d rotation_;
            Eigen::Matrix4d transformation_;
    };

    typedef std::shared_ptr<CameraOdomMeasurement> CameraOdomMeasurementPtr;
} // end husky_inekf namespace