//#include <dirent.h>
//#include <iostream>
//#include <vector>
//#include <string>
//#include <utility>
//
//#include <Eigen/Core>
//#include <Eigen/LU>
//#include <Eigen/Geometry>
//
//#include <opencv2/opencv.hpp>
//const int G_land_num = 74;
//const int G_train_pic_id_num = 3300;
//const int G_nShape = 47;
//const int G_nVerts = 11510;
//const int G_nFaces = 11540;
//const int G_test_num = 77;
//const int G_iden_num = 77;
//const int G_inner_land_num = 59;
//const int G_line_num = 50;
//const int G_jaw_land_num = 20;
//#define normalization
//#define win64
////#define linux
//
//#define cppio
//#define same_id
//
//
//struct Target_type {
//	Eigen::VectorXf exp;
//	Eigen::RowVector3f tslt;
//	Eigen::Matrix3f rot;
//	Eigen::MatrixX2f dis;
//
//};
//
//struct DataPoint
//{
//	cv::Mat image;
//	cv::Rect face_rect;
//	std::vector<cv::Point2d> landmarks;
//	//std::vector<cv::Point2d> init_shape;
//	Target_type shape, init_shape;
//	Eigen::VectorXf user;
//	Eigen::RowVector2f center;
//	Eigen::MatrixX2f land_2d;
//#ifdef posit
//	float f;
//#endif // posit
//#ifdef normalization
//	Eigen::MatrixX3f s;
//#endif
//
//	Eigen::VectorXi land_cor;
//};
//void load_bldshps(Eigen::MatrixXf &bldshps, std::string &name) {
//
//	puts("loading blendshapes...");
//	std::cout << name << std::endl;
//	FILE *fp;
//	fopen_s(&fp, name.c_str(), "rb");
//	for (int i = 0; i < G_iden_num; i++) {
//		for (int j = 0; j < G_nShape*G_nVerts * 3; j++)
//			fread(&bldshps(i, j), sizeof(float), 1, fp);
//	}
//	fclose(fp);
//}
//#ifdef cppio
//void get_mesh(Eigen::MatrixX3f& base_mesh, int mesh_vertex_num, std::string suffix[34469]) {
//#else
//void get_mesh(Eigen::MatrixX3f& base_mesh, int mesh_vertex_num, char suffix[34469][100]) {
//#endif // cppio
//
//
//
//#ifdef win64
//	std::string mesh_name = "D:/sydney/first/data/Tester_ (1)/TrainingPose/pose_0.obj";
//#endif // win64
//#ifdef linux
//	std::string mesh_name = "pose_0.obj";
//#endif // linux
//
//
//#ifdef cppio
//	std::ifstream op;
//	op.open(mesh_name);
//	base_mesh.resize(G_nVerts, 3);
//	for (int j = 0; j < mesh_vertex_num; j++) {
//		char c;
//		op >> c >> base_mesh(j, 0) >> base_mesh(j, 1) >> base_mesh(j, 2);
//	}
//
//	for (int j = mesh_vertex_num; j < 34469; j++)
//		getline(op, suffix[j]);
//
//	op.close();
//#else
//	FILE *fp;
//	puts("asd");
//	fopen_s(&fp, mesh_name.c_str(), "r");
//	puts("P");
//	base_mesh.resize(G_nVerts, 3);
//	char c;
//	//fscanf_s(fp, "%c", &c);
//	//putchar(c);
//	double p;
//	//fscanf_s(fp, "%lf", &p);
//	//printf("%.6f\n",p);
//	//fscanf_s(fp, "%lf", &p);
//	//printf("%.6f\n", p);
//	//fscanf_s(fp, "%lf", &p);
//	//printf("%.6f\n", p);
//	//fscanf_s(fp, "%c", &c);
//	//printf("ww%cqq\n-",c);
//	//fscanf_s(fp, "%c", &c);
//	//putchar(c);
//	//fscanf_s(fp, "%lf", &p);
//	//printf("%.6f\n", p);
//	//fscanf_s(fp, "%lf", &p);
//	//printf("%.6f\n", p);
//	//fscanf_s(fp, "%lf", &p);
//	//printf("%.6f\n", p);
//	//fscanf_s(fp, "%c", &c);
//	//printf("ww%cqq", c);
//	for (int j = 0; j < mesh_vertex_num; j++) {
//		/*char a,b,c;
//		float x, y, z;
//		fscanf_s(fp,"%c%lf%lf%lf%c",&a,&x,&y,&z,&b);
//
//		printf("%d %c %c %.6f %.6f %.6f\n", j,a,b, x, y, z);*/
//		fscanf_s(fp, "%c", &c);
//		putchar(c);
//		fscanf_s(fp, "%lf", &p);
//		printf("%.6f\n", p);
//		fscanf_s(fp, "%lf", &p);
//		printf("%.6f\n", p);
//		fscanf_s(fp, "%lf", &p);
//		printf("%d %.6f\n", j,p);
//		fscanf_s(fp, "%c", &c);
//		printf("ww%cqq", c);
//	}
//
//	for (int j = mesh_vertex_num; j < 34469; j++)
//		fgets(suffix[j], 98, fp);
//	fclose(fp);
//#endif // cppio
//	system("pause");
//
//
//}
//#ifdef cppio
//void print_mesh(Eigen::MatrixX3f &test_mesh_1, std::string suffix[34469], std::string name) {
//	std::ofstream out(name);
//
//	for (int i = 0; i < test_mesh_1.rows(); i++) {
//		out << "v " << test_mesh_1(i, 0) << ' ' << test_mesh_1(i, 1) << ' ' << test_mesh_1(i, 2) << '\n';
//	}
//	for (int i = test_mesh_1.rows() + 1; i < 34469; i++) {
//		out << suffix[i] << '\n';
//	}
//	out.close();
//}
//#else
//void print_mesh(Eigen::MatrixX3f &test_mesh_1, char suffix[34469][100], std::string name) {
//	FILE *fp;
//	fopen_s(&fp, name.c_str(), "w");
//
//
//	for (int i = 0; i < test_mesh_1.rows(); i++) {
//		fprintf(fp,"v %.6f %.6f %.6f\n", test_mesh_1(i, 0) , test_mesh_1(i, 1) , test_mesh_1(i, 2));
//		//out << "v " << test_mesh_1(i, 0) << ' ' << test_mesh_1(i, 1) << ' ' << test_mesh_1(i, 2) << '\n';
//	}
//	for (int i = test_mesh_1.rows() + 1; i < 34469; i++) {
//		fputs(suffix[i], fp);
//		//out << suffix[i] << '\n';
//	}
//
//	fclose(fp);
//}
//#endif // cppio
//
//
//
//
//void load_lv(std::string name, DataPoint &temp) {
//	std::cout << "load coefficients...file:" << name << "\n";
//	FILE *fp;
//	fopen_s(&fp, name.c_str(), "rb");
//
//	temp.user.resize(G_iden_num);
//	for (int j = 0; j < G_iden_num; j++)
//		fread(&temp.user(j), sizeof(float), 1, fp);
//	std::cout << temp.user << "\n";
//	//system("pause");
//	temp.land_2d.resize(G_land_num, 2);
//	for (int i_v = 0; i_v < G_land_num; i_v++) {
//		fread(&temp.land_2d(i_v, 0), sizeof(float), 1, fp);
//		fread(&temp.land_2d(i_v, 1), sizeof(float), 1, fp);
//	}
//
//
//	fread(&temp.center(0), sizeof(float), 1, fp);
//	fread(&temp.center(1), sizeof(float), 1, fp);
//
//	temp.shape.exp.resize(G_nShape);
//	for (int i_shape = 0; i_shape < G_nShape; i_shape++)
//		fread(&temp.shape.exp(i_shape), sizeof(float), 1, fp);
//
//	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
//		fread(&temp.shape.rot(i, j), sizeof(float), 1, fp);
//
//	for (int i = 0; i < 3; i++) fread(&temp.shape.tslt(i), sizeof(float), 1, fp);
//
//	temp.land_cor.resize(G_land_num);
//	for (int i_v = 0; i_v < G_land_num; i_v++) fread(&temp.land_cor(i_v), sizeof(int), 1, fp);
//
//	temp.s.resize(2, 3);
//	for (int i = 0; i < 2; i++) for (int j = 0; j < 3; j++)
//		fread(&temp.s(i, j), sizeof(float), 1, fp);
//
//	temp.shape.dis.resize(G_land_num, 2);
//	for (int i_v = 0; i_v < G_land_num; i_v++) {
//		fread(&temp.shape.dis(i_v, 0), sizeof(float), 1, fp);
//		fread(&temp.shape.dis(i_v, 1), sizeof(float), 1, fp);
//	}
//	std::cout << temp.shape.dis << "\n";
//	//system("pause");
//	fclose(fp);
//	puts("load successful!");
//}
//
//float cal_3d_vtx(
//	Eigen::MatrixXf &bldshps,
//	Eigen::VectorXf &user, Eigen::VectorXf &exp, int vtx_idx, int axis) {
//
//	//puts("calculating one vertex coordinate...");
//	float ans = 0;
//
//	for (int i_id = 0; i_id < G_iden_num; i_id++)
//		for (int i_shape = 0; i_shape < G_nShape; i_shape++)
//			if (i_shape == 0)
//				ans += exp(i_shape)*user(i_id)
//				*bldshps(i_id, vtx_idx * 3 + axis);
//			else
//				ans += exp(i_shape)*user(i_id)
//				*(bldshps(i_id, 3 * G_nVerts*i_shape + vtx_idx * 3 + axis) - bldshps(i_id, vtx_idx * 3 + axis));
//	return ans;
//}
//
//#ifdef cppio
//void solve(DataPoint &data, Eigen::MatrixXf &bldshps, std::string suffix[34469], std::string name) {
//#else
//void solve(DataPoint &data, Eigen::MatrixXf &bldshps, char suffix[34469][100], std::string name) {
//#endif
//
//
//
//	puts("calculating and saving mesh...");
//	std ::cout << "save obj name:" << name << "\n";
//	Eigen::MatrixX3f mesh(G_nVerts, 3);
//	Eigen::VectorXf user = data.user;
//	Eigen::VectorXf exp = data.shape.exp;
//	for (int i_v = 0; i_v < G_nVerts; i_v++) {
//		Eigen::Vector3f v;
//		for (int axis = 0; axis < 3; axis++)
//			v(axis) = cal_3d_vtx(bldshps, user, exp, i_v, axis);
//		mesh.row(i_v) = ((data.shape.rot) * v).transpose();
//	}
//	print_mesh(mesh, suffix, name);
//}
//int cnt = 0;
//#ifdef cppio
//void lv2mesh(std::string path, Eigen::MatrixXf &bldshps, std::string suffix[34469]) {
//#else
//void lv2mesh(std::string path, Eigen::MatrixXf &bldshps, char suffix[34469][100]) {
//#endif // cppio
//
//
//
//	struct dirent **namelist;
//	int n;
//	n = scandir(path.c_str(), &namelist, 0, alphasort);
//	if (n < 0)
//	{
//		std::cout << "scandir return " << n << "\n";
//		perror("Cannot open .");
//		exit(1);
//	}
//	else
//	{
//		int index = 0;
//		struct dirent *dp;
//		while (index < n)
//		{
//			dp = namelist[index];
//			std::cout << dp->d_name << ' ' << strlen(dp->d_name) << "\n";
//			if (dp->d_name[0] == '.') {
//				free(namelist[index]);
//				index++;
//				continue;
//			}
//			if (dp->d_type == DT_DIR) {
//				lv2mesh(path + "/" + dp->d_name,bldshps,suffix);
//			}
//			else {
//				int len = strlen(dp->d_name);
//				if (dp->d_name[len - 1] == 'v' && dp->d_name[len - 2] == 'l') {
//					////
//					std::string p = path + "/" + dp->d_name;
//					DataPoint data;
//					load_lv(p,data);
//					solve(data,bldshps,suffix, p.substr(0, p.find(".lv")) + "_0norm.obj");
//					std::cout << "cnt" << cnt << "\n";
//					cnt++;
//				}
//			}
//			free(namelist[index]);
//			index++;
//		}
//		free(namelist);
//	}
//
//}
//
//
//Eigen::MatrixXf bldshps(G_iden_num, G_nShape * 3 * G_nVerts);
//#ifdef win64
//std::string bldshps_path = "D:\\sydney\\first\\code\\2017\\deal_data_2\\deal_data/blendshape_ide_svd_77.lv";
//#endif // win64
//#ifdef linux
//std::string bldshps_path = "/home/weiliu/fitting_dde/cal/deal_data/blendshape_ide_svd_77.lv";
//#endif // linux
//
//#ifdef cppio
//std::string suffix[34469];
//#else
//char suffix[34469][100];
//#endif // cppio
//
//void cal_exp_r_t_all_matrix(
//	Eigen::MatrixXf &bldshps, DataPoint &data, Eigen::MatrixXf &result) {
//
//	puts("prepare exp_point matrix for bfgs/ceres...");
//	result.resize(G_nShape, 3 * G_nVerts);
//
//	for (int i_shape = 0; i_shape < G_nShape; i_shape++)
//		for (int i_v = 0; i_v < G_nVerts; i_v++) {
//			Eigen::Vector3f V;
//			V.setZero();
//			for (int j = 0; j < 3; j++)
//				for (int i_id = 0; i_id < G_iden_num; i_id++)
//					if (i_shape == 0)
//						V(j) += data.user(i_id)*bldshps(i_id, i_v * 3 + j);
//					else
//						V(j) += data.user(i_id)*
//						(bldshps(i_id, i_shape*G_nVerts * 3 + i_v * 3 + j) - bldshps(i_id, i_v * 3 + j));
//
//			for (int j = 0; j < 3; j++)
//				result(i_shape, i_v * 3 + j) = V(j);
//		}
//#ifdef deal_64
//	result.block(0, 64 * 3, G_nShape, 3).array() = (result.block(0, 59 * 3, G_nShape, 3).array() + result.block(0, 62 * 3, G_nShape, 3).array()) / 2;
//#endif // deal_64
//}
//
//void solve_same_id(DataPoint &data, Eigen::MatrixXf &exp_r_t_all_matrix, std::string suffix[34469], std::string name) {
//
//
//	puts("calculating and saving mesh...");
//	std::cout << "save obj name:" << name << "\n";
//	Eigen::MatrixX3f mesh(G_nVerts, 3);
//	Eigen::VectorXf user = data.user;
//	Eigen::VectorXf exp = data.shape.exp;
//	for (int i_v = 0; i_v < G_nVerts; i_v++) {
//		Eigen::Vector3f v;
//		v.setZero();
//		for (int axis = 0; axis < 3; axis++)
//			for (int i_shape = 0; i_shape < G_nShape; i_shape++)
//				v(axis) += data.shape.exp(i_shape)*exp_r_t_all_matrix(i_shape, i_v * 3 + axis);
//		mesh.row(i_v) = ((data.shape.rot) * v).transpose();
//	}
//	print_mesh(mesh, suffix, name);
//}
//void lv2mesh_same_id(std::string path, Eigen::MatrixXf &exp_r_t_all_matrix, std::string suffix[34469]) {
//	struct dirent **namelist;
//	int n;
//	n = scandir(path.c_str(), &namelist, 0, alphasort);
//	if (n < 0)
//	{
//		std::cout << "scandir return " << n << "\n";
//		perror("Cannot open .");
//		exit(1);
//	}
//	else
//	{
//		int index = 0;
//		struct dirent *dp;
//		while (index < n)
//		{
//			dp = namelist[index];
//			std::cout << dp->d_name << ' ' << strlen(dp->d_name) << "\n";
//			if (dp->d_name[0] == '.') {
//				free(namelist[index]);
//				index++;
//				continue;
//			}
//			if (dp->d_type == DT_DIR) {
//				lv2mesh(path + "/" + dp->d_name, bldshps, suffix);
//			}
//			else {
//				int len = strlen(dp->d_name);
//				if (dp->d_name[len - 1] == 'v' && dp->d_name[len - 2] == 'l') {
//					////
//					std::string p = path + "/" + dp->d_name;
//					DataPoint data;
//					load_lv(p, data);
//					solve_same_id(data, exp_r_t_all_matrix, suffix, p.substr(0, p.find(".lv")) + "_0norm.obj");
//					std::cout << "cnt" << cnt << "\n";
//					cnt++;
//				}
//			}
//			free(namelist[index]);
//			index++;
//		}
//		free(namelist);
//	}
//
//}
//
//
//int main() {
//
//	Eigen::MatrixX3f temp;
//	get_mesh(temp, G_nVerts, suffix);
//
//	load_bldshps(bldshps, bldshps_path);
//#ifdef same_id
//	DataPoint data;
//	load_lv("./lv_out/lv_out_1.lv", data);
//	Eigen::MatrixXf exp_r_t_all_matrix;
//	cal_exp_r_t_all_matrix(bldshps, data, exp_r_t_all_matrix);
//	lv2mesh_same_id("./lv_out/", exp_r_t_all_matrix, suffix);
//#else
//
//	lv2mesh("./data_one", bldshps, suffix);
//#endif // same_id
//	return 0;
//}
////g++ -Wall -std=c++11 `pkg-config --cflags opencv` -o deal main.cpp `pkg-config --libs opencv`


#include "vi-map/6dof-vi-map-gen.h"

#include <cmath>
#include <memory>
#include <vector>

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <posegraph/unique-id.h>
#include <sensors/imu.h>
#include <vi-map/pose-graph.h>
#include <vi-map/vertex.h>
#include <vi-map/vi-map.h>

#include "vi-map/6dof-pose-graph-gen.h"
#include "vi-map/6dof-test-trajectory-gen.h"
#include "vi-map/vi-optimization-test-helpers.h"

namespace vi_map {

void SixDofVIMapGenerator::generateVIMap() {
  graph_gen_.constructCamera();

  graph_gen_.settings_.num_of_landmarks = 500;
  setImuSigmasConstant(1e-10, &graph_gen_.settings_.imu_sigmas);

  graph_gen_.generatePathAndLandmarks();

  const size_t kNumOfVertices = 20;
  const int kNumOfFixedVertices = 1;
  const Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero();
  const Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero();
  const double kInitialPositionSigma = 0;
  const double kInitialRotationSigma = 0;

  graph_gen_.fillPosegraph(
      kNumOfVertices, kInitialPositionSigma, kInitialRotationSigma, gyro_bias,
      accel_bias, kNumOfFixedVertices);

  pose_graph::VertexIdList all_vertex_ids;
  pose_graph::EdgeIdList all_edge_ids;
  graph_gen_.posegraph_.getAllVertexIds(&all_vertex_ids);
  graph_gen_.posegraph_.getAllEdgeIds(&all_edge_ids);
  CHECK_EQ(kNumOfVertices, all_vertex_ids.size());
  CHECK_EQ(kNumOfVertices - 1, all_edge_ids.size());

  MissionId mission_id;
  common::generateId(&mission_id);

  const Vertex* vertex_ptr = static_cast<const Vertex*>(
      graph_gen_.posegraph_.getVertexPtr(all_vertex_ids[0]));

  pose::Transformation T_G_M;
  T_G_M = vertex_ptr->get_T_M_I();
  Eigen::Matrix<double, 6, 6> T_G_M_covariance;
  T_G_M_covariance.setIdentity();

  vi_map_.addNewMissionWithBaseframe(
      mission_id, T_G_M, T_G_M_covariance, graph_gen_.cameras_,
      Mission::BackBone::kViwls);

  constexpr char kImuHardwareId[] = "imu0";
  SensorId imu_sensor_id;
  common::generateId(&imu_sensor_id);
  Imu::UniquePtr imu_sensor = aligned_unique<Imu>(
      imu_sensor_id, static_cast<std::string>(kImuHardwareId));
  imu_sensor->setImuSigmas(graph_gen_.settings_.imu_sigmas);
  vi_map_.getSensorManager().addSensor(std::move(imu_sensor), mission_id);

  for (pose_graph::VertexId& vertex_id : all_vertex_ids) {
    Vertex* vertex_ptr = static_cast<Vertex*>(
        graph_gen_.posegraph_.getVertexPtrMutable(vertex_id));
    CHECK_NOTNULL(vertex_ptr);

    vertex_ptr->setMissionId(mission_id);
    pose_graph::EdgeIdSet incoming_edges;
    vertex_ptr->getIncomingEdges(&incoming_edges);

    Vertex::UniquePtr vertex_copy_ptr(new Vertex(*vertex_ptr));
    vi_map_.addVertex(std::move(vertex_copy_ptr));

    if (incoming_edges.empty()) {
      VIMission& mission = vi_map_.getMission(mission_id);
      mission.setRootVertexId(vertex_id);
    }
  }
  CHECK_EQ(vi_map_.numVertices(), kNumOfVertices);

  for (const pose_graph::EdgeId& edge_id : all_edge_ids) {
    ViwlsEdge* edge_ptr = static_cast<ViwlsEdge*>(
        graph_gen_.posegraph_.getEdgePtrMutable(edge_id));
    CHECK_NOTNULL(edge_ptr);

    ViwlsEdge::UniquePtr edge_copy_ptr(new ViwlsEdge(*edge_ptr));
    vi_map_.addEdge(std::move(edge_copy_ptr));
  }
  CHECK_EQ(vi_map_.numEdges(), kNumOfVertices - 1);

  // Add landmarks to the VIMap.
  for (const std::pair<LandmarkId, Landmark::Ptr>& landmark_pair :
       graph_gen_.landmarks_) {
    bool store_is_set = false;
    pose_graph::VertexId vertex_id_to_store;

    Landmark& landmark = *CHECK_NOTNULL(landmark_pair.second.get());

    if (!landmark_pair.first.isValid()) {
      continue;
    }
    CHECK_EQ(landmark_pair.first, landmark.id());

    if (landmark.numberOfObservations() == 0) {
      continue;
    }
    // Get the first observing vertex for this landmark.
    for (const KeypointIdentifier& backlink : landmark.getObservations()) {
      if (!store_is_set) {
        vertex_id_to_store = backlink.frame_id.vertex_id;
        store_is_set = true;
      }
      // Check that this landmark is in the global IDs for the vertex.
      if (vi_map_.getVertex(backlink.frame_id.vertex_id)
              .getObservedLandmarkId(
                  backlink.frame_id.frame_index, backlink.keypoint_index) !=
          landmark_pair.first) {
        LOG(ERROR) << "No existing reference to this keypoint.";
      }
    }
    Eigen::Vector3d G_p_fi = landmark.get_p_B();
    pose::Transformation G_T_I =
        vi_map_.getVertex(vertex_id_to_store).get_T_M_I();

    Eigen::Matrix<double, 3, 3> I_R_G;
    // I_q_G_passive is equal to G_q_I_active.
    Eigen::Quaterniond I_q_G_passive = G_T_I.getRotation().toImplementation();
    common::toRotationMatrixJPL(I_q_G_passive.coeffs(), &I_R_G);

    Eigen::Vector3d I_p_fi = G_T_I.inverse() * G_p_fi;
    landmark.set_p_B(I_p_fi);

    vi_map_.getVertex(vertex_id_to_store).getLandmarks().addLandmark(landmark);
    vi_map_.landmark_index.addLandmarkAndVertexReference(
        landmark_pair.first, vertex_id_to_store);
  }
}
Eigen::Vector3d SixDofVIMapGenerator::getTrueVertexPosition(
    const pose_graph::VertexId& vertex_id) const {
  return graph_gen_.true_vertex_positions_.at(vertex_id);
}
Eigen::Vector4d SixDofVIMapGenerator::getTrueVertexRotation(
    const pose_graph::VertexId& vertex_id) const {
  return graph_gen_.true_vertex_rotations_.at(vertex_id).coeffs();
}
}  // namespace vi_map


// An interval object.
#pragma once

#include <string>

#include <boost/numeric/interval.hpp>

#define USE_FILIB_INTERVALS
#ifdef USE_FILIB_INTERVALS
#include <interval/filib_rounding.hpp>
#endif

#include <utils/eigen_ext.hpp>

namespace ipc::rigid {

namespace interval_options {
    typedef boost::numeric::interval_lib::checking_catch_nan<double>
        CheckingPolicy;
} // namespace interval_options

#ifdef USE_FILIB_INTERVALS

// Use filib rounding arithmetic
typedef boost::numeric::interval<
    double,
    boost::numeric::interval_lib::policies<
        boost::numeric::interval_lib::save_state<FILibRounding>,
        interval_options::CheckingPolicy>>
    Interval;

#elif defined(__APPLE__)

// clang-format off
#warning "Rounding modes seem to be broken with trigonometric functions on macOS, unable to compute exact interval arithmetic!"
// clang-format on
typedef boost::numeric::interval<
    double,
    boost::numeric::interval_lib::policies<
        boost::numeric::interval_lib::save_state<
            boost::numeric::interval_lib::rounded_transc_exact<double>>,
        interval_options::CheckingPolicy>>
    Interval;

#else

// Use proper rounding arithmetic
typedef boost::numeric::interval<
    double,
    boost::numeric::interval_lib::policies<
        boost::numeric::interval_lib::save_state<
            boost::numeric::interval_lib::rounded_transc_std<double>>,
        interval_options::CheckingPolicy>>
    Interval;

#endif // USE_FILIB_INTERVALS

template <typename Derived>
inline Eigen::VectorXd width(const Eigen::MatrixBase<Derived>& x)
{
    Eigen::VectorXd w(x.size());
    for (int i = 0; i < x.size(); i++) {
        w(i) = width(x(i));
    }
    return w;
}

template <typename Derived>
inline double diagonal_width(const Eigen::MatrixBase<Derived>& x)
{
    Eigen::VectorXd widths = width(x);
    double w = 0;
    for (int i = 0; i < widths.size(); i++) {
        w += widths(i) * widths(i);
    }
    return sqrt(w);
}

template <typename Derived>
inline bool zero_in(const Eigen::MatrixBase<Derived>& x)
{
    // Check if the origin is in the n-dimensional interval
    for (int i = 0; i < x.size(); i++) {
        if (!boost::numeric::zero_in(x(i))) {
            return false;
        }
    }
    return true;
}

typedef Vector2<Interval> Vector2I;
typedef Vector3<Interval> Vector3I;
typedef VectorX<Interval> VectorXI;
typedef VectorMax3<Interval> VectorMax3I;
typedef Matrix3<Interval> Matrix2I;
typedef Matrix3<Interval> Matrix3I;
typedef MatrixMax3<Interval> MatrixMax3I;
typedef MatrixX<Interval> MatrixXI;

/// @brief Format a string for an Interval
std::string fmt_interval(const Interval& i, const int precision = 16);
/// @brief Format an eigen VectorX<Interval>
std::string fmt_eigen_intervals(const VectorXI& x, const int precision = 16);
} // namespace ipc::rigid

namespace Eigen {

template <typename BinOp>
struct ScalarBinaryOpTraits<ipc::rigid::Interval, double, BinOp> {
    typedef ipc::rigid::Interval ReturnType;
};

template <typename BinOp>
struct ScalarBinaryOpTraits<double, ipc::rigid::Interval, BinOp> {
    typedef ipc::rigid::Interval ReturnType;
};

#if EIGEN_MAJOR_VERSION >= 3
namespace internal {
    template <typename X, typename S, typename P>
    struct is_convertible<X, boost::numeric::interval<S, P>> {
        enum { value = is_convertible<X, S>::value };
    };

    template <typename S, typename P1, typename P2>
    struct is_convertible<
        boost::numeric::interval<S, P1>,
        boost::numeric::interval<S, P2>> {
        enum { value = true };
    };
} // namespace internal
#endif
} // namespace Eigen


// SPDX-License-Identifier: Apache-2.0
//
// Copyright 2011-2017 Ryan Curtin (http://www.ratml.org/)
// Copyright 2017 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <vector>
#include <armadillo>
#include "catch.hpp"

using namespace std;
using namespace arma;

/**
 * Make sure that gmm_full can fit manually constructed Gaussians.
 */
TEST_CASE("gmm_full_1")
  {
  // Higher dimensionality gives us a greater chance of having separated Gaussians.
  const uword dims      = 8;
  const uword gaussians = 3;
  const uword maxTrials = 3;

  // Generate dataset.
  mat data(dims, 500, fill::zeros);

  vector<vec>  means(gaussians);
  vector<mat> covars(gaussians);

  vec weights(gaussians);
  uvec counts(gaussians);

  bool success = true;

  for(uword trial = 0; trial < maxTrials; ++trial)
    {
    // Preset weights.
    weights[0] = 0.25;
    weights[1] = 0.325;
    weights[2] = 0.425;

    for(size_t i = 0; i < gaussians; i++)
      {
      counts[i] = data.n_cols * weights[i];
      }

    // Account for rounding errors (possibly necessary).
    counts[gaussians - 1] += (data.n_cols - accu(counts));

    // Build each Gaussian individually.
    size_t point = 0;
    for(size_t i = 0; i < gaussians; i++)
      {
      mat gaussian;
      gaussian.randn(dims, counts[i]);

      // Randomly generate mean and covariance.
      means[i].randu(dims);
      means[i] -= 0.5;
      means[i] *= (5 * i);

      // We need to make sure the covariance is positive definite.
      // We will take a random matrix C and then set our covariance to C * C',
      // which will be positive semidefinite.
      covars[i].randu(dims, dims);
      covars[i] += 0.5 * eye<mat>(dims, dims);
      covars[i] *= trans(covars[i]);

      data.cols(point, point + counts[i] - 1) = (covars[i] * gaussian + means[i] * ones<rowvec>(counts[i]));

      // Calculate the actual means and covariances
      // because they will probably be different.
      means[i] = mean(data.cols(point, point + counts[i] - 1), 1);
      covars[i] = cov(data.cols(point, point + counts[i] - 1).t(), 1 /* biased */);

      point += counts[i];
      }

    // Calculate actual weights.
    for(size_t i = 0; i < gaussians; i++)
      {
      weights[i] = (double) counts[i] / data.n_cols;
      }

    gmm_full gmm;
    gmm.learn(data, gaussians, eucl_dist, random_subset, 10, 500, 1e-10, false);

    uvec sortRef = sort_index(weights);
    uvec sortTry = sort_index(gmm.hefts);

    // Check the model to see that it is correct.
    success = ( gmm.hefts.n_elem == gaussians );
    for(size_t i = 0; i < gaussians; i++)
      {
      // Check weight.
      success &= ( weights[sortRef[i]] == Approx(gmm.hefts[sortTry[i]]).margin(0.1) );

      for(uword j = 0; j < gmm.means.n_rows; ++j)
        {
        success &= ( means[sortRef[i]][j] == Approx(gmm.means(j, sortTry[i])).margin(0.1) );
        }

      for(uword j = 0; j < gmm.fcovs.n_rows * gmm.fcovs.n_cols; ++j)
        {
        success &= ( covars[sortRef[i]][j] == Approx(gmm.fcovs.slice(sortTry[i])[j]).margin(0.1) );
        }

      if(success == false)  { continue; }
      }

    if(success)  { break; }
    }

  REQUIRE( success == true );
  }

TEST_CASE("gmm_diag_1")
  {
  // Higher dimensionality gives us a greater chance of having separated Gaussians.
  const uword dims      = 4;
  const uword gaussians = 3;
  const uword maxTrials = 8; // Needs more trials...

  // Generate dataset.
  mat data(dims, 500, fill::zeros);

  vector<vec>  means(gaussians);
  vector<mat> covars(gaussians);

  vec weights(gaussians);
  uvec counts(gaussians);

  bool success = true;
  for(uword trial = 0; trial < maxTrials; ++trial)
    {
    // Preset weights.
    weights[0] = 0.25;
    weights[1] = 0.325;
    weights[2] = 0.425;

    for(size_t i = 0; i < gaussians; i++)
      {
      counts[i] = data.n_cols * weights[i];
      }

    // Account for rounding errors (possibly necessary).
    counts[gaussians - 1] += (data.n_cols - accu(counts));

    // Build each Gaussian individually.
    size_t point = 0;
    for(size_t i = 0; i < gaussians; i++)
      {
      mat gaussian;
      gaussian.randn(dims, counts[i]);

      // Randomly generate mean and covariance.
      means[i].randu(dims);
      means[i] -= 0.5;
      means[i] *= (3 * (i + 1));

      // Use a diagonal covariance matrix.
      covars[i].zeros(dims, dims);
      covars[i].diag() = 0.5 * randu<vec>(dims) + 0.5;

      data.cols(point, point + counts[i] - 1) = (covars[i] * gaussian + means[i] * ones<rowvec>(counts[i]));

      // Calculate the actual means and covariances
      // because they will probably be different.
      means[i] = mean(data.cols(point, point + counts[i] - 1), 1);
      covars[i] = cov(data.cols(point, point + counts[i] - 1).t(), 1 /* biased */);

      point += counts[i];
      }

    // Calculate actual weights.
    for(size_t i = 0; i < gaussians; i++)
      {
      weights[i] = (double) counts[i] / data.n_cols;
      }

    gmm_diag gmm;
    gmm.learn(data, gaussians, eucl_dist, random_subset, 50, 500, 1e-10, false);

    uvec sortRef = sort_index(weights);
    uvec sortTry = sort_index(gmm.hefts);

    // Check the model to see that it is correct.
    success = ( gmm.hefts.n_elem == gaussians );
    for(size_t i = 0; i < gaussians; i++)
      {
      // Check weight.
      success &= ( weights[sortRef[i]] == Approx(gmm.hefts[sortTry[i]]).margin(0.1) );

      for(uword j = 0; j < gmm.means.n_rows; ++j)
        {
        success &= ( means[sortRef[i]][j] == Approx(gmm.means(j, sortTry[i])).margin(0.1) );
        }

      for(uword j = 0; j < gmm.dcovs.n_rows; ++j)
        {
        success &= ( covars[sortRef[i]](j, j) == Approx(gmm.dcovs.col(sortTry[i])[j]).margin(0.1) );
        }

      if(success == false)  { continue; }
      }

    if(success)  { break; }
    }

  REQUIRE( success == true );
  }


#ifndef RECURSION_SEARCH_HPP
#define RECURSION_SEARCH_HPP
/**
 * @file recursion_search.hpp
 *
 * @brief search parameters so that the random number generator's state
 * transition function has an irreducible characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2011 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>

namespace tinymt {
    /**
     * @class Search
     * search parameters so that the generator's state transition function
     * has an irreducible characteristic polynomial.
     * 1) call start() function.
     * 2) if start() returns true, then call get_random(), get_minpoly(),
     * or get_count().
     *
     * @tparam T generators class
     * @tparam SG sequential generator
     */
    template<class T, class SG> class Search {
    public:
        void get_minpoly(NTL::GF2X& minpoly, T& rand) {
            using namespace std;
            using namespace NTL;

            vec_GF2 vec;
            int mexp = rand.get_mexp();
            vec.SetLength(mexp * 2);
            for (int i = 0; i < mexp * 2; i++) {
                vec[i] = rand.generate() & 1;
            }
            MinPolySeq(minpoly, vec, mexp);
        }

        /**
         * generate random parameters and check if the generator's
         * state transition function has an irreducible characteristic
         * polynomial. If found in \b try_count times, return true, else
         * return false.
         *
         * @param try_count
         */
        bool start(int try_count) {
            long mexp = rand.get_mexp();
            long degree;
            for (int i = 0; i < try_count; i++) {
                rand.setup_param(sg->next());
                rand.seeding(1);
                get_minpoly(minpoly, rand);
                count++;
                degree = deg(minpoly);
                if (degree != mexp) {
                    continue;
                }
                if (IterIrredTest(minpoly)) {
                    return true;
                }
            }
            return false;
        }
        /**
         * call this function after \b start() has returned true.
         * @return random number generator class with parameters.
         */
        const T& get_random() const {
            return rand;
        }

        /**
         * call this function after \b start() has returned true.
         * In this program, if minimal polynomial is irreducible,
         * then the polynomial is characteristic polynomial of
         * generator's state transition function.
         *
         * @return minimal polynomial of generated sequence.
         */
        const NTL::GF2X& get_minpoly() const {
            return minpoly;
        }

        /**
         * @return tried count after this class has created.
         */
        int get_count() const {
            return count;
        }

        /**
         * @param rand_ random number generator whose parameters are
         * searched.
         * @param seq_generator a sequential number generator which
         * gives sequential number for searching parameters.
         *
         */
        Search(const T& rand_, SG& seq_generator) :
            rand(rand_)
            {
                sg = &seq_generator;
                count = 0;
            }
    private:
        T rand;
        NTL::GF2X minpoly;
        SG *sg;
        int count;
    };
}
#endif


#include "PolygonOffset2_EEK.h"
#include "PolygonOffset2.h"

#include <vector>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/create_offset_polygons_2.h>
#include <CGAL/create_straight_skeleton_from_polygon_with_holes_2.h>
#include <boost/shared_ptr.hpp>

void* PolygonOffset2_EEK_Create()
{
	return PolygonOffset2<EEK>::NewPolygonOffset2();
}

void PolygonOffset2_EEK_Release(void* ptr)
{
	PolygonOffset2<EEK>::DeletePolygonOffset2(ptr);
}

int PolygonOffset2_EEK_PolygonBufferSize(void* ptr)
{
	return PolygonOffset2<EEK>::PolygonBufferSize(ptr);
}

int PolygonOffset2_EEK_SegmentBufferSize(void* ptr)
{
	return PolygonOffset2<EEK>::SegmentBufferSize(ptr);
}

void PolygonOffset2_EEK_ClearPolygonBuffer(void* ptr)
{
	PolygonOffset2<EEK>::ClearPolygonBuffer(ptr);
}

void PolygonOffset2_EEK_ClearSegmentBuffer(void* ptr)
{
	PolygonOffset2<EEK>::ClearSegmentBuffer(ptr);
}

void* PolygonOffset2_EEK_GetBufferedPolygon(void* ptr, int index)
{
	return PolygonOffset2<EEK>::GetBufferedPolygon(ptr, index);
}

Segment2d PolygonOffset2_EEK_GetSegment(void* ptr, int index)
{
	return PolygonOffset2<EEK>::GetSegment(ptr, index);
}

void PolygonOffset2_EEK_GetSegments(void* ptr, Segment2d* segments, int count)
{
	PolygonOffset2<EEK>::GetSegments(ptr, segments, count);
}

void PolygonOffset2_EEK_CreateInteriorOffset(void* ptr, void* polyPtr, double offset)
{
	PolygonOffset2<EEK>::CreateInteriorOffset(ptr, polyPtr, offset);
}

void PolygonOffset2_EEK_CreateInteriorOffsetPWH(void* ptr, void* pwhPtr, double offset, BOOL boundaryOnly)
{
	PolygonOffset2<EEK>::CreateInteriorOffsetPWH(ptr, pwhPtr, offset, boundaryOnly);
}

void PolygonOffset2_EEK_CreateExteriorOffset(void* ptr, void* polyPtr, double offset)
{
	PolygonOffset2<EEK>::CreateExteriorOffset(ptr, polyPtr, offset);
}

void PolygonOffset2_EEK_CreateExteriorOffsetPWH(void* ptr, void* pwhPtr, double offset, BOOL boundaryOnly)
{
	PolygonOffset2<EEK>::CreateExteriorOffsetPWH(ptr, pwhPtr, offset, boundaryOnly);
}

void PolygonOffset2_EEK_CreateInteriorSkeleton(void* ptr, void* polyPtr, BOOL includeBorder)
{
	PolygonOffset2<EEK>::CreateInteriorSkeleton(ptr, polyPtr, includeBorder);
}

void PolygonOffset2_EEK_CreateInteriorSkeletonPWH(void* ptr, void* pwhPtr, BOOL includeBorder)
{
	PolygonOffset2<EEK>::CreateInteriorSkeletonPWH(ptr, pwhPtr, includeBorder);
}

void PolygonOffset2_EEK_CreateExteriorSkeleton(void* ptr, void* polyPtr, double maxOffset, BOOL includeBorder)
{
	PolygonOffset2<EEK>::CreateExteriorSkeleton(ptr, polyPtr, maxOffset, includeBorder);
}

void PolygonOffset2_EEK_CreateExteriorSkeletonPWH(void* ptr, void* pwhPtr, double maxOffset, BOOL includeBorder)
{
	PolygonOffset2<EEK>::CreateExteriorSkeletonPWH(ptr, pwhPtr, maxOffset, includeBorder);
}


#include "../src/tree.hpp"
#include <boost/test/unit_test.hpp>
#include <cstddef>
#include <random>

namespace data = n_body::data;
namespace logging = n_body::logging;
namespace space = n_body::space;
using Number = float;

BOOST_AUTO_TEST_SUITE(n_body_tree_test)

BOOST_AUTO_TEST_CASE(simple_tree_build) {

  constexpr std::size_t DIMENSION = 2;
  data::Bodies<Number, DIMENSION> bodies;
  auto create_body = [&bodies](Number mass, Number x, Number y) {
    bodies.push_back({
        .position =
            {
                x,
                y,
            },
        .velocity = {},
        .mass = mass,
    });
  };
  create_body(10, 1, 1);
  create_body(11, -1, -1);
  create_body(12, 1, -1);
  create_body(13, -1, 1);
  create_body(14, 0.1, 0.1);
  create_body(15, -0.1, -0.1);
  create_body(16, 0.1, -0.1);
  create_body(17, -0.1, 0.1);

  data::Space<Number, DIMENSION> root_space{.min =
                                                {
                                                    -1,
                                                    -1,
                                                },
                                            .max =
                                                {
                                                    1,
                                                    1,
                                                },
                                            .center = {
                                                0,
                                                0,
                                            }};

  data::tree::BodyTree<Number, DIMENSION> tree;
  for (size_t i = 0; i < bodies.size(); ++i) {
    tree.push(bodies, root_space, i);
  }

  std::size_t n = 0;
  BOOST_TEST(tree.node(n).mass == 108.f);
  {
    auto n0 = *tree.child_of_node(n, 0);
    BOOST_TEST(tree.node(n0).mass == 24.f);
    {
      auto n00 = *tree.child_of_node(n0, 0);
      BOOST_TEST(tree.body_of_node(n00) == 0);
      BOOST_TEST(tree.node(n00).mass == 10.f);
    }
    {
      auto n03 = *tree.child_of_node(n0, 3);
      BOOST_TEST(tree.body_of_node(n03) == 4);
      BOOST_TEST(tree.node(n03).mass == 14.f);
    }
  }
  {
    auto n1 = *tree.child_of_node(n, 1);
    BOOST_TEST(tree.node(n1).mass == 30.f);
    {
      auto n11 = *tree.child_of_node(n1, 1);
      BOOST_TEST(tree.body_of_node(n11) == 3);
      BOOST_TEST(tree.node(n11).mass == 13.f);
    }
    {
      auto n12 = *tree.child_of_node(n1, 2);
      BOOST_TEST(tree.body_of_node(n12) == 7);
      BOOST_TEST(tree.node(n12).mass == 17.f);
    }
  }
  {
    auto n2 = *tree.child_of_node(n, 2);
    BOOST_TEST(tree.node(n2).mass == 28.f);
    {
      auto n21 = *tree.child_of_node(n2, 1);
      BOOST_TEST(tree.body_of_node(n21) == 6);
      BOOST_TEST(tree.node(n21).mass == 16.f);
    }
    {
      auto n22 = *tree.child_of_node(n2, 2);
      BOOST_TEST(tree.body_of_node(n22) == 2);
      BOOST_TEST(tree.node(n22).mass == 12.f);
    }
  }
  {
    auto n3 = *tree.child_of_node(n, 3);
    BOOST_TEST(tree.node(n3).mass == 26.f);
    {
      auto n30 = *tree.child_of_node(n3, 0);
      BOOST_TEST(tree.body_of_node(n30) == 5);
      BOOST_TEST(tree.node(n30).mass == 15.f);
    }
    {
      auto n33 = *tree.child_of_node(n3, 3);
      BOOST_TEST(tree.body_of_node(n33) == 1);
      BOOST_TEST(tree.node(n33).mass == 11.f);
    }
  }
}

BOOST_AUTO_TEST_CASE(simple_merge) {
  constexpr std::size_t DIMENSION = 2;
  data::Bodies<Number, DIMENSION> bodies;
  auto create_body = [&bodies](Number mass, Number x, Number y) {
    bodies.push_back({
        .position =
            {
                x,
                y,
            },
        .velocity = {},
        .mass = mass,
    });
  };
  create_body(10, 1, 1);
  create_body(11, -1, -1);
  create_body(12, 1, -1);
  create_body(13, -1, 1);
  create_body(14, 0.1, 0.1);
  create_body(15, -0.1, -0.1);
  create_body(16, 0.1, -0.1);
  create_body(17, -0.1, 0.1);

  data::Space<Number, DIMENSION> root_space{.min =
                                                {
                                                    -1,
                                                    -1,
                                                },
                                            .max =
                                                {
                                                    1,
                                                    1,
                                                },
                                            .center = {
                                                0,
                                                0,
                                            }};

  for (std::size_t i = 0; i <= bodies.size(); ++i) {
    logging::logger(logging::Level::Trace)
        << "test merge tree 1 with size " << i << " and tree 2 with size "
        << bodies.size() - i << std::endl;
    data::tree::BodyTree<Number, DIMENSION> tree;
    data::tree::BodyTree<Number, DIMENSION> tree2;
    for (std::size_t j = 0; j < i; ++j) {
      tree.push(bodies, root_space, j);
    }
    for (std::size_t j = i; j < bodies.size(); ++j) {
      tree2.push(bodies, root_space, j);
    }
    tree.merge_tree(tree2, bodies);

    std::size_t n = 0;
    BOOST_TEST(tree.node(n).mass == 108.f);
    {
      auto n0 = *tree.child_of_node(n, 0);
      BOOST_TEST(tree.node(n0).mass == 24.f);
      {
        auto n00 = *tree.child_of_node(n0, 0);
        BOOST_TEST(tree.body_of_node(n00) == 0);
        BOOST_TEST(tree.node(n00).mass == 10.f);
      }
      {
        auto n03 = *tree.child_of_node(n0, 3);
        BOOST_TEST(tree.body_of_node(n03) == 4);
        BOOST_TEST(tree.node(n03).mass == 14.f);
      }
    }
    {
      auto n1 = *tree.child_of_node(n, 1);
      BOOST_TEST(tree.node(n1).mass == 30.f);
      {
        auto n11 = *tree.child_of_node(n1, 1);
        BOOST_TEST(tree.body_of_node(n11) == 3);
        BOOST_TEST(tree.node(n11).mass == 13.f);
      }
      {
        auto n12 = *tree.child_of_node(n1, 2);
        BOOST_TEST(tree.body_of_node(n12) == 7);
        BOOST_TEST(tree.node(n12).mass == 17.f);
      }
    }
    {
      auto n2 = *tree.child_of_node(n, 2);
      BOOST_TEST(tree.node(n2).mass == 28.f);
      {
        auto n21 = *tree.child_of_node(n2, 1);
        BOOST_TEST(tree.body_of_node(n21) == 6);
        BOOST_TEST(tree.node(n21).mass == 16.f);
      }
      {
        auto n22 = *tree.child_of_node(n2, 2);
        BOOST_TEST(tree.body_of_node(n22) == 2);
        BOOST_TEST(tree.node(n22).mass == 12.f);
      }
    }
    {
      auto n3 = *tree.child_of_node(n, 3);
      BOOST_TEST(tree.node(n3).mass == 26.f);
      {
        auto n30 = *tree.child_of_node(n3, 0);
        BOOST_TEST(tree.body_of_node(n30) == 5);
        BOOST_TEST(tree.node(n30).mass == 15.f);
      }
      {
        auto n33 = *tree.child_of_node(n3, 3);
        BOOST_TEST(tree.body_of_node(n33) == 1);
        BOOST_TEST(tree.node(n33).mass == 11.f);
      }
    }
  }
}

template <typename T, std::size_t Dimension>
void compare_subtree(const data::tree::BodyTree<T, Dimension> &tree1,
                     std::size_t root1,
                     const data::tree::BodyTree<T, Dimension> &tree2,
                     std::size_t root2) {
  BOOST_TEST(tree1.node(root1).node_type() == tree2.node(root2).node_type());
  BOOST_TEST(tree1.node(root1).mass == tree2.node(root2).mass);
  // BOOST_TEST(tree1.node(root1).center_of_mass == tree2.node(root2).center_of_mass);
  if (tree1.node(root1).node_type() == data::tree::NodeType::Inner) {
    for (std::size_t i = 0;
         i < data::tree::BodyTreeInnerNode<T, Dimension>::CHILDREN_NUMBER;
         ++i) {
      if (!tree1.child_of_node(root1, i) && !tree2.child_of_node(root2, i)) {
        continue;
      } else if (tree1.child_of_node(root1, i) &&
                 tree2.child_of_node(root2, i)) {
        compare_subtree(tree1, *tree1.child_of_node(root1, i), tree2,
                        *tree2.child_of_node(root2, i));
      } else {
        BOOST_FAIL("node missing");
      }
    }
  } else {
    // node type is leaf
    BOOST_TEST(tree1.body_of_node(root1) == tree2.body_of_node(root2));
  }
}

template <typename T, std::size_t Dimension>
void compare_tree(const data::tree::BodyTree<T, Dimension> &tree1,
                  const data::tree::BodyTree<T, Dimension> &tree2) {
  if (tree1.tree.empty() && tree2.tree.empty()) {
    return;
  } else if (!tree1.tree.empty() && !tree2.tree.empty()) {
    return compare_subtree(tree1, 0, tree2, 0);
  } else {
    BOOST_FAIL("node missing");
  }
}

BOOST_AUTO_TEST_CASE(huge_merge) {

  constexpr std::size_t DIMENSION = 3;
  constexpr std::size_t NUMBER = 100;
  data::Bodies<Number, DIMENSION> bodies;
  std::random_device rd;
  auto dist_pos = std::normal_distribution<Number>(-1000.f, 1000.f);
  auto dist_mass = std::uniform_int_distribution(1, 10);
  for (std::size_t i = 0; i < NUMBER; ++i) {
    bodies.push_back({
        .position =
            {
                dist_pos(rd),
                dist_pos(rd),
                dist_pos(rd),
            },
        .velocity = {},
        .mass = static_cast<Number>(dist_mass(rd)),
    });
  }

  data::Space<Number, DIMENSION> root_space =
      space::root_space<Number, DIMENSION>(bodies.begin(), bodies.end());

  for (std::size_t i = 0; i <= bodies.size(); ++i) {
    logging::logger(logging::Level::Trace)
        << "test merge tree 1 with size " << i << " and tree 2 with size "
        << bodies.size() - i << std::endl;
    data::tree::BodyTree<Number, DIMENSION> tree;
    data::tree::BodyTree<Number, DIMENSION> tree_merged;
    data::tree::BodyTree<Number, DIMENSION> part;
    for (std::size_t j = 0; j < bodies.size(); ++j) {
      tree.push(bodies, root_space, j);
    }
    for (std::size_t j = 0; j < i; ++j) {
      tree_merged.push(bodies, root_space, j);
    }
    for (std::size_t j = i; j < bodies.size(); ++j) {
      part.push(bodies, root_space, j);
    }
    tree_merged.merge_tree(part, bodies);

    // compare tree and tree_merged
    compare_tree(tree, tree_merged);
  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <type_traits>
#include <cassert>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/multiplies.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/transform.hpp>

namespace mpl = boost::mpl;
using namespace boost::mpl::placeholders;

int main()
{
    typedef mpl::vector_c<int, 1, 2, 3> t1;
    typedef mpl::vector_c<int, 1, 4, 9> t3;
    typedef typename mpl::transform<t1, mpl::multiplies<_1, _1>>::type t2;
    // static_assert((std::is_same<t2::type, t3::type>::value), "same or not?");
    return 0;
}


#include <trajopt_sco/modeling.hpp>
#include <trajopt_sco/modeling_utils.hpp>
#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_utils/eigen_conversions.hpp>
#include <iostream>
#include <Eigen/Eigenvalues>

using namespace std;
using namespace util;

namespace sco {

const double DEFAULT_EPSILON = 1e-5;

VectorXd getVec(const vector<double>& x, const VarVector& vars) {
  VectorXd out(vars.size());
  for (unsigned i=0; i < vars.size(); ++i) out[i] = x[vars[i].var_rep->index];
  return out;
}

DblVec getDblVec(const vector<double>& x, const VarVector& vars) {
  DblVec out(vars.size());
  for (unsigned i=0; i < vars.size(); ++i) out[i] = x[vars[i].var_rep->index];
  return out;
}

AffExpr affFromValGrad(double y, const VectorXd& x, const VectorXd& dydx, const VarVector& vars) {
  AffExpr aff;
  aff.constant = y - dydx.dot(x);
  aff.coeffs = toDblVec(dydx);
  aff.vars = vars;
  aff = cleanupAff(aff);
  return aff;
}

CostFromFunc::CostFromFunc(ScalarOfVectorPtr f, const VarVector& vars, const string& name, bool full_hessian) :
  Cost(name), f_(f), vars_(vars), full_hessian_(full_hessian), epsilon_(DEFAULT_EPSILON) {}

double CostFromFunc::value(const vector<double>& xin) {
  VectorXd x = getVec(xin, vars_);
  return f_->call(x);
}

ConvexObjectivePtr CostFromFunc::convex(const vector<double>& xin, Model* model) {
  VectorXd x = getVec(xin, vars_);

  ConvexObjectivePtr out(new ConvexObjective(model));
  if (!full_hessian_) {
    double val;
    VectorXd grad,hess;
    calcGradAndDiagHess(*f_, x, epsilon_, val, grad, hess);
    hess = hess.cwiseMax(VectorXd::Zero(hess.size()));
    QuadExpr& quad = out->quad_;
    quad.affexpr.constant = val - grad.dot(x) + .5*x.dot(hess.cwiseProduct(x));
    quad.affexpr.vars = vars_;
    quad.affexpr.coeffs = toDblVec(grad - hess.cwiseProduct(x));
    quad.vars1 = vars_;
    quad.vars2 = vars_;
    quad.coeffs = toDblVec(hess*.5);
  }
  else {
    double val;
    VectorXd grad;
    MatrixXd hess;
    calcGradHess(f_, x, epsilon_, val, grad, hess);

    MatrixXd pos_hess = MatrixXd::Zero(x.size(), x.size());
    Eigen::SelfAdjointEigenSolver<MatrixXd> es(hess);
    VectorXd eigvals = es.eigenvalues();
    MatrixXd eigvecs = es.eigenvectors();
    for (size_t i=0, end = x.size(); i != end; ++i) { //tricky --- eigen size() is signed
      if (eigvals(i) > 0) pos_hess += eigvals(i) * eigvecs.col(i) * eigvecs.col(i).transpose();
    }

    QuadExpr& quad = out->quad_;
    quad.affexpr.constant = val - grad.dot(x) + .5*x.dot(pos_hess * x);
    quad.affexpr.vars = vars_;
    quad.affexpr.coeffs = toDblVec(grad - pos_hess * x);

    int nquadterms = (x.size() * (x.size()-1))/2;
    quad.coeffs.reserve(nquadterms);
    quad.vars1.reserve(nquadterms);
    quad.vars2.reserve(nquadterms);
    for (size_t i=0, end = x.size(); i != end; ++i) { //tricky --- eigen size() is signed
      quad.vars1.push_back(vars_[i]);
      quad.vars2.push_back(vars_[i]);
      quad.coeffs.push_back(pos_hess(i,i)/2);
      for (size_t j=i+1; j != end; ++j) {  //tricky --- eigen size() is signed
        quad.vars1.push_back(vars_[i]);
        quad.vars2.push_back(vars_[j]);
        quad.coeffs.push_back(pos_hess(i,j));
      }
    }
  }

  return out;
}

CostFromErrFunc::CostFromErrFunc(VectorOfVectorPtr f, const VarVector& vars, const VectorXd& coeffs, PenaltyType pen_type, const std::string& name) :
    Cost(name), f_(f), vars_(vars), coeffs_(coeffs), pen_type_(pen_type), epsilon_(DEFAULT_EPSILON) {}
CostFromErrFunc::CostFromErrFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, const VectorXd& coeffs, PenaltyType pen_type, const std::string& name) :
    Cost(name), f_(f), dfdx_(dfdx), vars_(vars), coeffs_(coeffs), pen_type_(pen_type), epsilon_(DEFAULT_EPSILON) {}
double CostFromErrFunc::value(const vector<double>& xin) {
  VectorXd x = getVec(xin, vars_);
  VectorXd err = f_->call(x);
  if (coeffs_.size()>0) err.array() *= coeffs_.array();
  switch (pen_type_) {
    case SQUARED: return err.array().square().sum();
    case ABS: return err.array().abs().sum();
    case HINGE: return err.cwiseMax(VectorXd::Zero(err.size())).sum();
    default: assert(0 && "unreachable");
  }

  return 0; // avoid compiler warning
}
ConvexObjectivePtr CostFromErrFunc::convex(const vector<double>& xin, Model* model) {
  VectorXd x = getVec(xin, vars_);
  MatrixXd jac = (dfdx_) ? dfdx_->call(x) : calcForwardNumJac(*f_, x, epsilon_);
  ConvexObjectivePtr out(new ConvexObjective(model));
  VectorXd y = f_->call(x);
  for (int i=0; i < jac.rows(); ++i) {
    AffExpr aff = affFromValGrad(y[i], x, jac.row(i), vars_);
    if (coeffs_.size()>0) {
      exprScale(aff, coeffs_[i]);
      if (coeffs_[i] == 0) continue;
    }
    switch (pen_type_) {
      case SQUARED: out->addQuadExpr(exprSquare(aff)); break;
      case ABS: out->addAbs(aff, 1); break;
      case HINGE: out->addHinge(aff, 1); break;
      default: assert(0 && "unreachable");
    }
  }
  return out;
}

ConstraintFromFunc::ConstraintFromFunc(VectorOfVectorPtr f, const VarVector& vars, const VectorXd& coeffs, ConstraintType type, const std::string& name) :
    Constraint(name), f_(f), vars_(vars), coeffs_(coeffs), type_(type), epsilon_(DEFAULT_EPSILON) {}

ConstraintFromFunc::ConstraintFromFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, const VectorXd& coeffs, ConstraintType type, const std::string& name) :
    Constraint(name), f_(f), dfdx_(dfdx), vars_(vars), coeffs_(coeffs), type_(type), epsilon_(DEFAULT_EPSILON) {}

vector<double> ConstraintFromFunc::value(const vector<double>& xin) {
  VectorXd x = getVec(xin, vars_);
   VectorXd err = f_->call(x);
   if (coeffs_.size()>0) err.array() *= coeffs_.array();
   return toDblVec(err);
}

ConvexConstraintsPtr ConstraintFromFunc::convex(const vector<double>& xin, Model* model) {
  VectorXd x = getVec(xin, vars_);
  MatrixXd jac = (dfdx_) ? dfdx_->call(x) : calcForwardNumJac(*f_, x, epsilon_);
  ConvexConstraintsPtr out(new ConvexConstraints(model));
  VectorXd y = f_->call(x);
  for (int i=0; i < jac.rows(); ++i) {
    AffExpr aff = affFromValGrad(y[i], x, jac.row(i), vars_);
    if (coeffs_.size()>0) {
      if (coeffs_[i] == 0) continue;
      exprScale(aff, coeffs_[i]);
    }
    if (type() == INEQ) out->addIneqCnt(aff);
    else out->addEqCnt(aff);
  }
  return out;
}

}


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

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

 * See LICENSE for the license information

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

/**
 * @file testGaussianJunctionTree.cpp
 * @date Jul 8, 2010
 * @author Kai Ni
 */

#include <iostream>
#include <CppUnitLite/TestHarness.h>

#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;

#include <gtsam/base/debug.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/BayesTree-inl.h>

using namespace std;
using namespace gtsam;

static const Index x2=0, x1=1, x3=2, x4=3;

static GaussianFactorGraph createChain() {

  typedef GaussianFactorGraph::sharedFactor Factor;
  SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5);
  Factor factor1(new JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.),  model));
  Factor factor2(new JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.),  model));
  Factor factor3(new JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.),  model));
  Factor factor4(new JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.),  model));

  GaussianFactorGraph fg;
  fg.push_back(factor1);
  fg.push_back(factor2);
  fg.push_back(factor3);
  fg.push_back(factor4);

  return fg;
}

/* ************************************************************************* */
/**
 * x1 - x2 - x3 - x4
 * x3 x4
 *    x2 x1 : x3
 *
 * x2 x1 x3 x4  b
 *  1  1        1
 *  1     1     1
 *        1  1  1
 *           1  1
 *
 *  1  0  0  1
 */
TEST( GaussianJunctionTree, eliminate )
{
  GaussianFactorGraph fg = createChain();
  GaussianJunctionTree junctionTree(fg);
  BayesTree<GaussianConditional>::sharedClique rootClique = junctionTree.eliminate(&EliminateQR);

  typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
  Matrix two = Matrix_(1,1,2.);
  Matrix one = Matrix_(1,1,1.);

  BayesTree<GaussianConditional> bayesTree_expected;
  Index keys_root[] = {x3,x4};
  Matrix rsd_root = Matrix_(2,3, 2., 2., 2., 0., 2., 2.);
  size_t dim_root[] = {1, 1, 1};
  sharedConditional root_expected(new GaussianConditional(keys_root, keys_root+2, 2,
      VerticalBlockView<Matrix>(rsd_root, dim_root, dim_root+3, 2), ones(2)));
  BayesTree<GaussianConditional>::sharedClique rootClique_expected(new BayesTree<GaussianConditional>::Clique(root_expected));

  Index keys_child[] = {x2,x1,x3};
  Matrix rsd_child = Matrix_(2,4, 2., 1., 1., 2., 0., -1., 1., 0.);
  size_t dim_child[] = {1, 1, 1, 1};
  sharedConditional child_expected(new GaussianConditional(keys_child, keys_child+3, 2,
      VerticalBlockView<Matrix>(rsd_child, dim_child, dim_child+4, 2), ones(2)));
  BayesTree<GaussianConditional>::sharedClique childClique_expected(new BayesTree<GaussianConditional>::Clique(child_expected));

  bayesTree_expected.insert(rootClique_expected);
  bayesTree_expected.insert(childClique_expected);

//  bayesTree_expected.insert(sharedConditional(new GaussianConditional(x4, Vector_(1,2.), two, Vector_(1,1.))));
//  bayesTree_expected.insert(sharedConditional(new GaussianConditional(x3, Vector_(1,2.), two, x4, two, Vector_(1,1.))));
//  bayesTree_expected.insert(sharedConditional(new GaussianConditional(x1, Vector_(1,0.), one*(-1), x3, one, Vector_(1,1.))));
//  bayesTree_expected.insert(sharedConditional(new GaussianConditional(x2, Vector_(1,2.), two, x1, one, x3, one, Vector_(1,1.))));
  CHECK(assert_equal(*bayesTree_expected.root(), *rootClique));
  EXPECT(assert_equal(*(bayesTree_expected.root()->children().front()), *(rootClique->children().front())));
}

/* ************************************************************************* */
TEST( GaussianJunctionTree, GBNConstructor )
{
  GaussianFactorGraph fg = createChain();
  GaussianJunctionTree jt(fg);
  BayesTree<GaussianConditional>::sharedClique root = jt.eliminate(&EliminateQR);
  BayesTree<GaussianConditional> expected;
  expected.insert(root);

  GaussianBayesNet bn(*GaussianSequentialSolver(fg).eliminate());
  BayesTree<GaussianConditional> actual(bn);

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( GaussianJunctionTree, optimizeMultiFrontal )
{
  GaussianFactorGraph fg = createChain();
  GaussianJunctionTree tree(fg);

  VectorValues actual = tree.optimize(&EliminateQR);
  VectorValues expected(vector<size_t>(4,1));
  expected[x1] = Vector_(1, 0.);
  expected[x2] = Vector_(1, 1.);
  expected[x3] = Vector_(1, 0.);
  expected[x4] = Vector_(1, 1.);
  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST(GaussianJunctionTree, complicatedMarginal) {

  // Create the conditionals to go in the BayesTree
  GaussianConditional::shared_ptr R_1_2(new GaussianConditional(
      pair_list_of
          (1, (Matrix(3,1) <<
              0.2630,
              0,
              0).finished())
          (2, (Matrix(3,2) <<
              0.7482,    0.2290,
              0.4505,    0.9133,
                   0,    0.1524).finished())
          (5, (Matrix(3,1) <<
              0.8258,
              0.5383,
              0.9961).finished()),
      2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished(), ones(3)));
  GaussianConditional::shared_ptr R_3_4(new GaussianConditional(
      pair_list_of
          (3, (Matrix(3,1) <<
              0.0540,
              0,
              0).finished())
          (4, (Matrix(3,2) <<
              0.9340,    0.4694,
              0.1299,    0.0119,
                   0,    0.3371).finished())
          (6, (Matrix(3,2) <<
              0.1622,    0.5285,
              0.7943,    0.1656,
              0.3112,    0.6020).finished()),
      2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished(), ones(3)));
//  GaussianConditional::shared_ptr R_5_6(new GaussianConditional(
//      pair_list_of
//          (5, (Matrix(3,1) <<
//              0.2435,
//              0,
//              0).finished())
//          (6, (Matrix(3,2) <<
//              0.4733,    0.1966,
//              0.9022,    0.0979,
//                 0.0,    0.2312).finished())     // Attempted to recreate without permutation
//          (7, (Matrix(3,1) <<
//              0.5853,
//              1.0589,
//              0.1487).finished())
//          (8, (Matrix(3,2) <<
//              0.2858,    0.3804,
//              0.9893,    0.2912,
//              0.4035,    0.4933).finished()),
//      2, (Vector(3) << 0.8173, 0.4164, 0.7671).finished(), ones(3)));
  GaussianConditional::shared_ptr R_5_6(new GaussianConditional(
      pair_list_of
          (5, (Matrix(3,1) <<
              0.2435,
              0,
              0).finished())
          (6, (Matrix(3,2) <<
              0.4733,    0.1966,
              0.3517,    0.2511,
              0.8308,    0.0).finished()) // NOTE the non-upper-triangular form
              // here since this test was written when we had column permutations
              // from LDL.  The code still works currently (does not enfore
              // upper-triangularity in this case) but this test will need to be
              // redone if this stops working in the future
          (7, (Matrix(3,1) <<
              0.5853,
              0.5497,
              0.9172).finished())
          (8, (Matrix(3,2) <<
              0.2858,    0.3804,
              0.7572,    0.5678,
              0.7537,    0.0759).finished()),
      2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished(), ones(3)));
  GaussianConditional::shared_ptr R_7_8(new GaussianConditional(
      pair_list_of
          (7, (Matrix(3,1) <<
              0.2551,
              0,
              0).finished())
          (8, (Matrix(3,2) <<
              0.8909,    0.1386,
              0.9593,    0.1493,
                   0,    0.2575).finished())
          (11, (Matrix(3,1) <<
              0.8407,
              0.2543,
              0.8143).finished()),
      2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished(), ones(3)));
  GaussianConditional::shared_ptr R_9_10(new GaussianConditional(
      pair_list_of
          (9, (Matrix(3,1) <<
              0.7952,
              0,
              0).finished())
          (10, (Matrix(3,2) <<
              0.4456,    0.7547,
              0.6463,    0.2760,
                   0,    0.6797).finished())
          (11, (Matrix(3,1) <<
              0.6551,
              0.1626,
              0.1190).finished())
          (12, (Matrix(3,2) <<
              0.4984,    0.5853,
              0.9597,    0.2238,
              0.3404,    0.7513).finished()),
      2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished(), ones(3)));
  GaussianConditional::shared_ptr R_11_12(new GaussianConditional(
      pair_list_of
          (11, (Matrix(3,1) <<
              0.0971,
              0,
              0).finished())
          (12, (Matrix(3,2) <<
              0.3171,    0.4387,
              0.9502,    0.3816,
                   0,    0.7655).finished()),
      2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished(), ones(3)));

  // Gaussian Bayes Tree
  typedef BayesTree<GaussianConditional> GaussianBayesTree;
  typedef GaussianBayesTree::Clique Clique;
  typedef GaussianBayesTree::sharedClique sharedClique;

  // Create Bayes Tree
  GaussianBayesTree bt;
  bt.insert(sharedClique(new Clique(R_11_12)));
  bt.insert(sharedClique(new Clique(R_9_10)));
  bt.insert(sharedClique(new Clique(R_7_8)));
  bt.insert(sharedClique(new Clique(R_5_6)));
  bt.insert(sharedClique(new Clique(R_3_4)));
  bt.insert(sharedClique(new Clique(R_1_2)));

  // Marginal on 5
  Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
  JacobianFactor::shared_ptr actualJacobianChol= boost::dynamic_pointer_cast<JacobianFactor>(
      bt.marginalFactor(5, EliminateCholesky));
  JacobianFactor::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
      bt.marginalFactor(5, EliminateQR));
  CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
  LONGS_EQUAL(1, actualJacobianChol->rows());
  LONGS_EQUAL(1, actualJacobianChol->size());
  LONGS_EQUAL(5, actualJacobianChol->keys()[0]);
  Matrix actualA = actualJacobianChol->getA(actualJacobianChol->begin());
  Matrix actualCov = inverse(actualA.transpose() * actualA);
  EXPECT(assert_equal(expectedCov, actualCov, 1e-1));

  // Marginal on 6
//  expectedCov = (Matrix(2,2) <<
//      8471.2, 2886.2,
//      2886.2, 1015.8).finished();
  expectedCov = (Matrix(2,2) <<
      1015.8,    2886.2,
      2886.2,    8471.2).finished();
  actualJacobianChol = boost::dynamic_pointer_cast<JacobianFactor>(
      bt.marginalFactor(6, EliminateCholesky));
  actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
      bt.marginalFactor(6, EliminateQR));
  CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
  LONGS_EQUAL(2, actualJacobianChol->rows());
  LONGS_EQUAL(1, actualJacobianChol->size());
  LONGS_EQUAL(6, actualJacobianChol->keys()[0]);
  actualA = actualJacobianChol->getA(actualJacobianChol->begin());
  actualCov = inverse(actualA.transpose() * actualA);
  EXPECT(assert_equal(expectedCov, actualCov, 1e1));

}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


#include "catch.hpp"
#include <Eigen/core>
#include <squanch/Model.h>
#include <squanch/IgaAlgorithms.h>
#include <squanch/Circle.h>
#include <squanch/RectangleXy.h>
#include <squanch/RuledSurface.h>

TEST_CASE("Ruled surface", "[ruled-surface]")
{
  SECTION("circle_inside_out")
  {
    using namespace squanch;
    using namespace Eigen;

    squanch::Model<double> model;

    const double pi = 3.14159265359;

    Circle<double, Eigen::Vector3d> circle1;
    circle1.start_angle = 0.0;
    circle1.end_angle = 2 * pi;
    circle1.N1 = Eigen::Vector3d(1, 0, 0);
    circle1.N2 = Eigen::Vector3d(0, 1, 0);
    circle1.origin = Eigen::Vector3d(0, 0, 0);
    circle1.r = 5.0;
    auto&& ci = circle1.to_nurbs(model);

    Circle<double, Eigen::Vector3d> circle2;
    circle2.start_angle = 0.0;
    circle2.end_angle = 2 * pi;
    circle2.N1 = Eigen::Vector3d(1, 0, 0);
    circle2.N2 = Eigen::Vector3d(0, 1, 0);
    circle2.origin = Eigen::Vector3d(0, 0, 0);
    circle2.r = 10.0;
    auto&& co = circle2.to_nurbs(model);

    auto&& area = create_ruled_surface(model, ci, co);

  }

  SECTION("circle_inside_rectangle")
  {
    using namespace squanch;
    using namespace Eigen;

    squanch::Model<double> model;

    const double pi = 3.14159265359;

    Circle<double, Eigen::Vector3d> circle1;
    circle1.start_angle = 0.0;
    circle1.end_angle = 2 * pi;
    circle1.N1 = Eigen::Vector3d(1, 0, 0);
    circle1.N2 = Eigen::Vector3d(0, 1, 0);
    circle1.origin = Eigen::Vector3d(10, 10, 0);
    circle1.r = 5.0;
    auto&& ci = circle1.to_nurbs(model);

    RectangleXy<double> rect(20, 20);

    auto&& co = rect.to_nurbs(model);

    auto&& area = create_ruled_surface(model, ci, co);

  }
}


//==================================================================================================
/*!
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <utility>
#include <array>

template<class T>
auto foo(T & t) -> typename std::enable_if<std::is_scalar<T>::value, decltype(boost::simd::sqr(t))>::type
{
  return boost::simd::sqr(t);
}

template<class T>
auto foo(T &t) -> typename std::enable_if<!std::is_scalar<T>::value, T &>::type
{
  for(std::size_t i = 0; i < t.size(); ++i)
    t[i] = foo(t[i]);
  return t;
}

int main()
{
  std::array<int, 3> data = {{1,2,3}};
  foo(data);

  return 0;
}


/*******************************************************************************
 *
 *  Filename    : KinematicCompare
 *  Description : Plotting function for Implementation
 *  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
 *
*******************************************************************************/
#include "TstarAnalysis/CompareDataMC/interface/SampleHistMgr.hh"
#include "TstarAnalysis/CompareDataMC/interface/FileNames.hh"
#include "TCanvas.h"
#include "TLegend.h"
#include "THStack.h"
#include "TColor.h"
#include "TLatex.h"
#include "TLine.h"

#include <boost/range/adaptor/reversed.hpp>

using namespace std;

void MakeComparePlot(
   SampleHistMgr* all_data,
   vector<SampleHistMgr*>& background,
   SampleHistMgr* signal_mgr,
   const string label = "" )
{
   const double total_lumi = SampleHistMgr::TotalLuminosity();

   background[0]->SetColor( TColor::GetColor("#FFCC88") );
   background[1]->SetColor( TColor::GetColor("#996600") );
   background[2]->SetColor( TColor::GetColor("#FF3333") );
   background[3]->SetColor( TColor::GetColor("#33EEEE") );
   background[4]->SetColor( TColor::GetColor("#0066EE") );

   for( const auto& hist_name : background.front()->AvailableHistList() ){
      const unsigned bins = background.front()->GetHist(hist_name)->GetXaxis()->GetNbins();
      const double   xmin = background.front()->GetHist(hist_name)->GetXaxis()->GetXmin();
      const double   xmax = background.front()->GetHist(hist_name)->GetXaxis()->GetXmax();
      THStack* stack      = new THStack("stack","");
      TH1D*    error_hist = new TH1D("error","",bins,xmin,xmax);
      TH1D*    data_hist  = new TH1D("data" ,"",bins,xmin,xmax);
      Parameter error(0,0,0);

      // Making stack and total error hist
      for( auto& sample : boost::adaptors::reverse(background) ){
         stack->Add( sample->GetHist( hist_name ), "HIST" );
         error_hist->Add( sample->GetHist(hist_name) );
         error += sample->ExpectedYield();
      }

      // Making data hist
      data_hist->Add( all_data->GetHist(hist_name) );

      // Making relative error plot
      TH1D*  data_rel_hist = (TH1D*)data_hist->Clone("data_rel");
      TH1D*  rel_error     = (TH1D*)error_hist->Clone("rel_error");
      data_rel_hist->Divide( error_hist );
      double avgError = error.RelAvgError();
      for( int i = 0 ; i < error_hist->GetSize() ; ++i ){
         double binContent   = error_hist->GetBinContent(i);
         double binError     = error_hist->GetBinError(i);
         double dataBinError = data_hist->GetBinError(i);
         binError += binContent*avgError;
         error_hist->SetBinError(i,binError);

         rel_error->SetBinContent(i,1.0);
         if( binContent != 0 ){
            rel_error->SetBinError(i, binError/binContent);
            data_rel_hist->SetBinError(i,dataBinError/binContent);
         } else {
            rel_error->SetBinError(i,0.0);
            data_rel_hist->SetBinError(i,0.0);
         }
      }
      // Making Signal plots
      TH1D*    signal_hist   = signal_mgr->GetHist(hist_name);

      // Legend settings
      TLegend* l = new TLegend(0.62,0.3,0.90,0.90);
      char data_entry[128];
      sprintf( data_entry , "Data (%lg fb^{-1})" , total_lumi/1000. );
      l->AddEntry( data_hist , data_entry , "lp" );
      l->AddEntry( background[0]->GetHist(hist_name), "t#bar{t} + Jets"  , "f" );
      l->AddEntry( background[1]->GetHist(hist_name), "Single top"       , "f" );
      l->AddEntry( background[2]->GetHist(hist_name), "t#bar{t} + Boson" , "f");
      l->AddEntry( background[3]->GetHist(hist_name), "Single Boson"     , "f" );
      l->AddEntry( background[4]->GetHist(hist_name), "Di-Boson"         , "f" );
      l->AddEntry( error_hist , "Background error" , "fl" );
      l->AddEntry( signal_hist, "t^{*}#bar{t}^{*} {}_{M_{t^{*}}=700GeV} (10 pb)" , "fl");

      // The Plotting commands
      TCanvas* c = new TCanvas("c","c",650,500);

      // Drawing the top canvas
      TPad* pad1 = new TPad( "pad1" , "pad1" , 0 , 0.30 , 1., 1.0 );
      pad1->SetBottomMargin(0.050);
      pad1->Draw();
      pad1->cd();
      stack->Draw("HIST");
      data_hist->Draw("same LPE1");
      error_hist->Draw("same E2");
      signal_hist->Draw("same HIST");
      l->Draw("same");
      c->cd();

      // Drawing bottom canvas
      TPad*   pad2 = new TPad( "pad2" , "pad2" , 0 , 0.10, 1, 0.30 );
      TLine*  line = new TLine( error_hist->GetXaxis()->GetXmin() , 1 , error_hist->GetXaxis()->GetXmax() , 1 );
      pad2->SetTopMargin(0.025);
      pad2->SetBottomMargin(0.010);
      pad2->Draw();
      pad2->cd();
      data_rel_hist->Draw("axis");
      data_rel_hist->Draw("same LPE1");
      rel_error->Draw("same E2");
      line->Draw("same");
      c->cd();

      // Plot Settings
      error_hist->SetFillColor(kBlack);
      error_hist->SetLineColor(kBlack);
      error_hist->SetFillStyle(3004);
      rel_error->SetFillColor(kBlack);
      rel_error->SetLineColor(kBlack);
      rel_error->SetFillStyle(3004);
      rel_error->SetStats(0);
      data_hist->SetMarkerStyle(21);
      data_hist->SetLineColor(kBlack);
      data_hist->SetStats(0);
      data_rel_hist->SetMarkerStyle(21);
      data_rel_hist->SetLineColor(kBlack);
      data_rel_hist->SetStats(0);
      signal_hist->SetLineColor(kRed);
      signal_hist->SetFillColor(kRed);
      signal_hist->SetFillStyle(3005);

      // Font and title settings
      const size_t font_size = 14;
      stack->GetXaxis()->SetLabelSize(0);
      stack->GetYaxis()->SetLabelFont(43);
      stack->GetYaxis()->SetLabelSize( font_size );
      stack->GetYaxis()->SetTitleFont(43);
      stack->GetYaxis()->SetTitleSize( font_size );
      stack->GetYaxis()->SetTitle( signal_hist->GetYaxis()->GetTitle() );
      stack->GetYaxis()->SetTitleOffset(1.2);
      data_rel_hist->GetXaxis()->SetTitle( signal_hist->GetXaxis()->GetTitle() );
      data_rel_hist->GetXaxis()->SetLabelFont(43);
      data_rel_hist->GetXaxis()->SetLabelSize( font_size );
      data_rel_hist->GetYaxis()->SetLabelFont(43);
      data_rel_hist->GetXaxis()->SetTitleFont(43);
      data_rel_hist->GetXaxis()->SetTitleSize( font_size );
      data_rel_hist->GetXaxis()->SetTitleOffset(5.5);
      data_rel_hist->GetYaxis()->SetLabelSize( font_size );
      data_rel_hist->GetYaxis()->SetTitle( "#frac{Data}{MC}");
      data_rel_hist->GetYaxis()->SetTitleFont(43);
      data_rel_hist->GetYaxis()->SetTitleSize( font_size );
      data_rel_hist->GetYaxis()->SetTitleOffset(1.2);
      data_rel_hist->SetMaximum(2.0);
      data_rel_hist->SetMinimum(0.0);
      l->SetTextSizePixels( font_size );

      // Additional captions
      TLatex tl;
      tl.SetTextFont(43);
      tl.SetTextSize( font_size + 4 );

      tl.SetTextAlign(11);
      tl.DrawLatex( 0.1, 0.95 , "CMS at #sqrt{s} = 13TeV");

      tl.SetTextAlign(31);
      string channel_msg = "("+GetChannelPlotLabel() + " " + label+")";
      tl.DrawLatex( 0.9, 0.95 , channel_msg.c_str() );

      c->SaveAs( GetKinematicPlotFile(hist_name+"_"+label).c_str() );

      delete pad1;
      delete pad2;
      delete c;
      delete l;
      delete data_hist;
      delete error_hist;
      delete stack;
      delete line;
   }

}


#include <boost/math/distributions/non_central_f.hpp>


#include <boost/graph/distributed/dijkstra_shortest_paths.hpp>


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

#include <Utils/Math/BSplines/BSpline.h>
#include <Utils/Math/BSplines/BSplineTools.h>
#include <Utils/Math/BSplines/ControlPolygonGenerator.h>
#include <gmock/gmock.h>
#include <Eigen/Core>

using namespace testing;
namespace Scine {
namespace Utils {
using namespace BSplines;
namespace Tests {

class ABSplineToolsTest : public Test {
 public:
  unsigned p;
  BSpline bs;

  unsigned l;
  unsigned idx;

  void SetUp() override {
  }
};

TEST_F(ABSplineToolsTest, LeftEqualCorrectIdDegree1) {
  p = 1;

  Eigen::MatrixXd data(3, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 ,0.5, 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 |

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(1u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(1u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));
}

TEST_F(ABSplineToolsTest, LeftCorrectIdDegree1) {
  p = 1;

  Eigen::MatrixXd data(3, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 ,0.5, 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 |

  l = BSplineTools::findIdxOfLeftDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(1u));

  l = BSplineTools::findIdxOfLeftDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(1u));

  l = BSplineTools::findIdxOfLeftDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(1u));

  l = BSplineTools::findIdxOfLeftDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));

  l = BSplineTools::findIdxOfLeftDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));
}

TEST_F(ABSplineToolsTest, RightCorrectIdDegree1) {
  p = 1;

  Eigen::MatrixXd data(3, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 ,0.5, 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 |

  l = BSplineTools::findIdxOfRightDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));

  l = BSplineTools::findIdxOfRightDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));

  l = BSplineTools::findIdxOfRightDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfRightDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfRightDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));
}

TEST_F(ABSplineToolsTest, RightOrEqualCorrectIdDegree1) {
  p = 1;

  Eigen::MatrixXd data(3, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 ,0.5, 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 |

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(1u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(2u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));
}

TEST_F(ABSplineToolsTest, LeftEqualCorrectIdDegree3) {
  p = 3;

  Eigen::MatrixXd data(5, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);
  data.row(3) = Eigen::Vector2d(4, 0);
  data.row(4) = Eigen::Vector2d(5, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 , 0 , 0 ,0.5, 1 , 1 , 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 |

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));

  l = BSplineTools::findIdxOfLeftOrEqualDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(5u));
}

TEST_F(ABSplineToolsTest, LeftCorrectIdDegree3) {
  p = 3;

  Eigen::MatrixXd data(5, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);
  data.row(3) = Eigen::Vector2d(4, 0);
  data.row(4) = Eigen::Vector2d(5, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 , 0 , 0 ,0.5, 1 , 1 , 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 |

  l = BSplineTools::findIdxOfLeftDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfLeftDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfLeftDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfLeftDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));

  l = BSplineTools::findIdxOfLeftDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));
}

TEST_F(ABSplineToolsTest, RightCorrectIdDegree3) {
  p = 3;

  Eigen::MatrixXd data(5, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);
  data.row(3) = Eigen::Vector2d(4, 0);
  data.row(4) = Eigen::Vector2d(5, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 , 0 , 0 ,0.5, 1 , 1 , 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 |

  l = BSplineTools::findIdxOfRightDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));

  l = BSplineTools::findIdxOfRightDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));

  l = BSplineTools::findIdxOfRightDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(5u));

  l = BSplineTools::findIdxOfRightDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(5u));

  l = BSplineTools::findIdxOfRightDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(5u));
}

TEST_F(ABSplineToolsTest, RightOrEqualCorrectIdDegree3) {
  p = 3;

  Eigen::MatrixXd data(5, 2);
  data.row(0) = Eigen::Vector2d(1, 1);
  data.row(1) = Eigen::Vector2d(2, 0);
  data.row(2) = Eigen::Vector2d(3, 1);
  data.row(3) = Eigen::Vector2d(4, 0);
  data.row(4) = Eigen::Vector2d(5, 1);

  ControlPolygonGenerator myBSplineGenerator_(data, p, true);
  bs = myBSplineGenerator_.generateBSpline();

  // U =  { 0 , 0 , 0 , 0 ,0.5, 1 , 1 , 1 , 1 };
  //      | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 |

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(3u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.4999, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.5, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(4u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(0.5001, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(5u));

  l = BSplineTools::findIdxOfRightOrEqualDomainKnot(1.0, p, bs.getKnotVector());
  ASSERT_THAT(l, Eq(5u));
}

} // namespace Tests
} // namespace Utils
} // namespace Scine