// Note, from test.cpp we have the solver to work without segfault issues for upto 8000 such Point Structs
// And, here we don't have segfault upto 6000 points
// So continue with the solver for 4k points instead of 40k points. Create a 4k version and run the solver
// Match the Julia and C++m outputs for 4k points now, and then you can think about 40k points later, what say?

#include<stdio.h>
#include<iostream>
#include<math.h>
#include <unistd.h>
#include <armadillo>
// #include <json/json.h>
// #include <json/value.h>
#include <fstream>
#include<map>
#include<string>
#include<iomanip>
#include<limits>
#include<string>
#include<regex>
#include <sstream>

#define ARMA_DONT_PRINT_ERRORS
using namespace arma;

typedef std::map<char, float> char_float_map;
typedef std::map<char, double> char_double_map;
typedef std::vector<std::string> vec_str;
typedef std::vector<double> vec_doub;
typedef std::vector<long double> vec_ldoub;
//typedef std::numeric_limits< double > dbl;

bool debug_mode = true;

struct Point
{
	int localID;
	double x, y;
	int left, right;
	int flag_1, flag_2; // Int8 in the Julia code
	double short_distance;
	int nbhs;
	int conn[20];
	double nx, ny;
	// Size 4 (Pressure, vx, vy, density) x numberpts
	double prim[4];
	double flux_res[4];
	double q[4];
	// Size 2(x,y) 4(Pressure, vx, vy, density) numberpts
	double dq1[4];
	double dq2[4];
	double entropy;
	int xpos_nbhs, xneg_nbhs, ypos_nbhs, yneg_nbhs;
	int xpos_conn[20];
	int xneg_conn[20];
	int ypos_conn[20];
	int yneg_conn[20];
	double delta;
	double max_q[4];
	double min_q[4];
	double prim_old[4];

	//Point Constructor

	Point() {}

};

struct Config
{
	struct Core
	{
		int points;
		double cfl;
		int max_iters;
		double mach;
		double aoa;
		double power;
		int limiter_flag;
		double vl_const;
		int initial_conditions_flag;
		int interior_points_normal_flag;
		double shapes;
		double rho_inf;
		double pr_inf;
		int threadsperblock;
		double gamma;
		int clcd_flag;

		Core() {}

	};

	struct PointConfig
	{
		int wall, interior, outer;

		PointConfig() {}
	};

	struct Format
	{
		char* type;

		Format() {}
	};

	Core core = Core();
	PointConfig point_config = PointConfig();
	Format format = Format();

	Config() {}

	void setConfig()
	{
		core.points = 9600;
		core.cfl = 0.5;
		core.max_iters = 10;
		core.mach = 1.2;
		core.aoa = 0.0;
		core.power = 0.0;
		core.limiter_flag = 0;
		core.vl_const = 200.0;
		core.initial_conditions_flag = 0;
		core.interior_points_normal_flag = 0;
		core.shapes = 0.0;
		core.rho_inf = 0.0;
		core.pr_inf = 0.7142857142857142;
		core.threadsperblock = 128;
		core.gamma = 1.4;
		core.clcd_flag = 0;

		point_config.wall = 0;
		point_config.interior = 1;
		point_config.outer = 2;

		format.type = "quadtree";

	}

	void printConfig()
	{
		cout<<"Core points: "<<core.points<<endl;
		cout<<"Core cfl: "<<core.cfl<<endl;
		cout<<"Core max_iters: "<<core.max_iters<<endl;
		cout<<"Core mach: "<<core.mach<<endl;
		cout<<"Core aoa: "<<core.aoa<<endl;
		cout<<"Core power: "<<core.power<<endl;
		cout<<"Core limiter_flag: "<<core.limiter_flag<<endl;
		cout<<"Core vl_const: "<<core.vl_const<<endl;
		cout<<"Core initial_conditions_flag: "<<core.initial_conditions_flag<<endl;
		cout<<"Core interior_points_normal_flag: "<<core.interior_points_normal_flag<<endl;
		cout<<"Core shapes: "<<core.shapes<<endl;
		cout<<"Core rho_inf: "<<core.rho_inf<<endl;
		cout<<"Core pr_inf: "<<core.pr_inf<<endl;
		cout<<"Core threadsperblock: "<<core.threadsperblock<<endl;
		cout<<"Core gamma: "<<core.gamma<<endl;
		cout<<"Core clcd_flag: "<<core.clcd_flag<<endl;

		cout<<"Config wall: "<<point_config.wall<<endl;
		cout<<"Config interior: "<<point_config.interior<<endl;
		cout<<"Config outer: "<<point_config.outer<<endl;

		cout<<"Format type: "<<format.type<<endl;

	}
};

void meshfree_solver(char* file_name, int num_iters);
//void getConfig(Json::Value &config);
//void set_manual_config(std::map<char, char_float_map> &config);
void getInitialPrimitive(Config configData, double primal[4]);
void printPrimal(double primal[4]);
double calculateTheta(Config configData);
std::string readFile(char* file_name);

int main(int argc, char **argv)
{
	printf("Meshfree AD\n");

	/* initialize random seed*/
	srand (time(NULL));
	arma_rng::set_seed_random();

	meshfree_solver(argv[1], (int)argv[2]); //Warning: Casting from char to int loses precision
	//Gotta see if maintaining a global array id efficient or if passing around by reference is efficient
	//For all we know, maintaining a global data structure instead of passing it around might be more efficient
}

#if 0

void getConfig(Json::Value &config)
{
	std::ifstream config_file("../config.json", std::ifstream::binary);
	config_file>>config;
	cout<<config;
	config["format"] = "quadtree";
}

void set_manual_config(std::map<char, char_double_map> &config)
{
	char_double_map core_config =
	{
		{'1', 9600},
        {'2', 0.5},
        {'3', 10},
        {'4', 1.2},
        {'5', 0.0},
        {'6', 0.0},
        {'7', 1},
        {'8', 200.0},
        {'9', 0},
        {'A', 0},
        {'B', 1.0},
        {'C', 1.0},
        {'D', 0.7142857142857142},
        {'E', 128},
        {'F', 1.4},
        {'G', 0}
	};

	char_double_map point_config =
	{
		{'1', 0},
		{'2', 1},
		{'3', 2}
	}; // Gotta typecast these values back to int, or use templating appropriately later.

	char_double_map format_config =
	{
		{'1', 1} // Type 1 refers to Quadtree, Type 0 refers to non-quadtree
	};

	config =
	{
	    { '1', core_config },
	    { '2', point_config },
	    { '3', format_config }
	};

For reference:
{
    "core":{
        "points":9600,  // 1
        "cfl": 0.5, // 2
        "max_iters": 10, // 3
        "mach": 1.2, // 4
        "aoa": 0.0, // 5
        "power": 0.0, // 6
        "limiter_flag": 1, // 7
        "vl_const": 200.0, // 8
        "initial_conditions_flag": 0, // 9
        "interior_points_normal_flag": 0, // 10
        "shapes": 1.0, // 11
        "rho_inf": 1.0, // 12
        "pr_inf": 0.7142857142857142, // 13
        "threadsperblock": 128, // 14
        "gamma": 1.4, // 15
        "clcd_flag": 0 // 16
    },
    "point":{
        "wall": 0,
        "interior": 1,
        "outer": 2
    },
    "format":{
        "type":"quadtree"
    }
}

}

#endif

std::string readFile(char* file_name)
{
	std::string gridfile, tp;

	std::fstream datafile(file_name, ios::in);
	if(datafile.is_open())
	{
		cout<<"File opened"<<endl;
		//read the file
		while(getline(datafile, tp))
		{
			//cout<<"Data File: \n"<<tp<<endl;
			gridfile.append(tp);
			gridfile.append("\n");
		}
	}
	//cout<<"Grid file: \n"<<gridfile<<endl;
	datafile.close();

	return gridfile;
}

inline double deg2rad(double radians) {
    return radians * (180.0 / M_PI);
}

double calculateTheta(Config configData)
{
	double theta = deg2rad(configData.core.aoa);
	return theta;
}

void getInitialPrimitive(Config configData, double primal[4])
{
	primal[0] = configData.core.rho_inf;
	double mach = configData.core.mach;
	double machcos = mach * cos(calculateTheta(configData));
	double machsin = mach * sin(calculateTheta(configData));
	primal[1] = machcos;
	primal[2] = machsin;
	primal[3] = configData.core.pr_inf;
}

void printPrimal(double primal[4])
{
	cout<<"Printing Primal: "<<endl;
	for(int i=0; i<4; i++)
		cout<<std::fixed<<std::setprecision(20)<<primal[i]<<" ";
	cout<<"\n";
}

void meshfree_solver(char* file_name, int max_iters)
{
	//cout << std::setprecision(18);
	//Json::Value configData = getConfig(Json::Value configData); // Assuming config file is located at "../config.json"
	//std::cout << std::setprecision(17); // Show 17 digits of precision
	//cout.precision(17);

#if 0

	std::map<char, char_double_map> configData;
	set_manual_config(configData);

	if(configData['1']['F'] == 1.45)
		cout<<"All good"<<endl;
	else
		cout<<"Values Changed"<<endl;
	std::cout<<"Checking config read: "<<endl;
	cout<<"gamma "<<configData['1']['F']<<endl;

	std::streamsize ss = std::cout.precision();
	cout.precision(dbl::max_digits10);
	cout<<std::fixed<<"pr_inf "<<configData['1']['D']<<endl; // Shows 0.71428...29. Why am I getting the extra 9

	cout.precision(ss);
	cout<<"gamma "<<configData['1']['F']<<endl;
	cout<<"threadsperblock "<<configData['1']['E']<<endl;

	if(configData['1']['F'] == 1.39999997615814209)
		cout<<"Values Changed"<<endl;
	else
		cout<<"All good"<<endl;

	// For some reason this double works well but float doesn't
	// Please note that setprecision only changes what we display through cout, the internal representation used for comparisons anol is still ABSOLUTELY THE SAME.

	double gamma = 1.4;
	cout<<gamma<<endl;

	double pr_inf = 0.7142857142857142;
	cout<<pr_inf<<endl;

	if(gamma == 1.4)
		cout<<"All good"<<endl;
	else
		cout<<"Values Changed"<<endl;

	if(pr_inf == 0.7142857142857142)
		cout<<"All good"<<endl;
	else
		cout<<"Values Changed"<<endl;

	cout.precision(dbl::max_digits10);

	cout<<pr_inf<<endl;

	if(gamma == 1.4)
		cout<<"All good"<<endl;
	else
		cout<<"Values Changed"<<endl;

	if(pr_inf == 0.7142857142857142)
		cout<<"All good"<<endl;
	else
		cout<<"Values Changed"<<endl;

#endif

	Config configData = Config();
	configData.setConfig();
	if (debug_mode)
		configData.printConfig();

	std::string format = configData.format.type;
	if (debug_mode)
		cout<<"Format: "<<format<<endl;

	cout<<"Filename: "<<file_name<<endl;

	// fmat data;
	// bool load_ok = data.load(file_name);
	// //int end = 1;
	// //int numPoints = data.submat(0, 0, 0, end);
	// if (debug_mode)
	// {
	// 	cout<<"No. of rows: "<<data.n_rows<<endl;
	// 	cout<<"No. of columns: "<<data.n_cols<<endl;
	// }

	//std::string gridfile = readFile(file_name);
	//cout<<gridfile[0]<<endl<<gridfile[1]<<endl<<gridfile[2]<<endl;
	int numPoints = 0;
	std::fstream datafile(file_name, ios::in);
	// if(datafile.is_open())
	// {
	// 	cout<<"File opened"<<endl;
	// 	for(int i=0; i<5; i++)
	// 	{
	// 		getline(datafile, new_file[i]);
	// 	}
	// }
	// for(int i=0; i<5; i++)
	// {
	// 	cout<<new_file[i]<<endl;
	// }
	if(datafile.is_open())
	{
		std::string temp;
		getline(datafile, temp);
		std::stringstream num(temp);
		num >> numPoints;
	}

	std::string new_file[numPoints], temp;
	if(datafile.is_open())
	{
		for(int i=0; i<numPoints; i++)
		{
			getline(datafile, new_file[i]);
		}
	}
	datafile.close();

	//cout<<new_file[1][0]<<endl;
	cout<<"No. of points: "<<numPoints<<endl;

	std::regex ws_re("\\s+");
	std::vector<vec_str> result;
	for(int i=0; i<numPoints; i++)  // This might include the first line as well so, you need to store that separately or just throw the 1st line away (the one that contains the file length)
									// There are 48738 lines apart from that
	{
    	std::vector<std::string> temp{std::sregex_token_iterator(new_file[i].begin(), new_file[i].end(), ws_re, -1), {}};
    	result.push_back(temp);
    }
#if 0

    for(int j=0; j<numPoints; j++)
    {
		for (int i=0; i<result[j].size(); i++)
			cout<<"Result: "<<j<<" " <<i<<" "<<result[j][i]<<endl;

	}

#endif

	std::vector<vec_doub> result_doub;
	for(int j=0; j<numPoints; j++)
    {
		vec_doub temp;
		for (int i=0; i<result[j].size(); i++)
			temp.push_back(std::stod(result[j][i]));
		result_doub.push_back(temp);

	}
#if 0
	for(int j=0; j<20; j++)
    {
		for (int i=0; i<result_doub[j].size(); i++)
			cout<<std::fixed<<std::setprecision(20)<<"Result Doub: "<<j<<" " <<i<<" "<<result_doub[j][i]<<endl;

	}
#endif

	// std::vector<vec_ldoub> result_ldoub;
	// for(int j=0; j<5; j++)
 //    {
	// 	vec_ldoub temp;
	// 	for (int i=0; i<result[j].size(); i++)
	// 		temp.push_back(std::stold(result[j][i]));
	// 	result_ldoub.push_back(temp);

	// }

	// for(int j=0; j<5; j++)
 //    {
	// 	for (int i=0; i<result_ldoub[j].size(); i++)
	// 		cout<<"Result LDoub: "<<j<<" " <<i<<" "<<result_ldoub[j][i]<<endl;

	// }

	// //long double check = std::stold(result[4][9]);
	// double check = std::stod(result[4][9]);
	// cout<<std::fixed<<std::setprecision(25)<<"Check: "<<check<<endl;
	// cout<<std::fixed<<std::setprecision(22)<<"Check 2: "<<check<<endl;
	// cout<<std::fixed<<std::setprecision(20)<<"Check 3: "<<check<<endl<<endl;

	// double checkz = std::stod(result[3][9]);
	// cout<<std::fixed<<std::setprecision(25)<<"Check: "<<checkz<<endl;
	// cout<<std::fixed<<std::setprecision(22)<<"Check 2: "<<checkz<<endl;
	// cout<<std::fixed<<std::setprecision(20)<<"Check 3: "<<checkz<<endl;

	Point* globaldata = new Point[numPoints]; // it gives seg fault even for 7000 points if statically allocated
	double res_old = 0.0;
	double main_store[62] = {0};

	double defprimal[4];
	//cout<<"hi"<<endl;
	getInitialPrimitive(configData, defprimal);
	printPrimal(defprimal);

	cout<<"Start Read: "<<endl; // readFile function in Julia

	for(int i=0; i<numPoints; i++)
	{
		if(i==0) continue; // The first line contains the file length

		int connectivity[20] = {0};

	}

	// Interior, Out and Wall were defined as Int64 in Julia, so am defining them as long long

	long long interior = configData.point_config.interior;
	long long wall = configData.point_config.wall;
	long long outer = configData.point_config.outer;

}


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

/* Author: Ioan Sucan */

#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/kpiece/KPIECE1.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/util/PPM.h>
#include <string>
#include <ompl/config.h>
#include <../tests/resources/config.h>

#include <boost/filesystem.hpp>
#include <iostream>

#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/objectives/StateCostIntegralObjective.h>
#include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
// For ompl::msg::setLogLevel
#include "ompl/util/Console.h"

// For boost program options
#include <boost/program_options.hpp>
// For string comparison (boost::iequals)
#include <boost/algorithm/string.hpp>
// For std::make_shared
#include <memory>
#include <cmath>
#include <fstream>

namespace ob = ompl::base;
namespace og = ompl::geometric;

class Plane2DEnvironment
{
public:

	Plane2DEnvironment(const char *ppm_file)
	{
		bool ok = false;
		try
		{
			ppm_.loadFile(ppm_file);
			ok = true;
		}
		catch (ompl::Exception &ex)
		{
			OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
		}
		if (ok)
		{
			maxWidth_ = ppm_.getWidth() - 1;
			maxHeight_ = ppm_.getHeight() - 1;

		}
	}

	void plan(unsigned int start_row, unsigned int start_col, double start_yaw, unsigned int goal_row, unsigned int goal_col, double goal_yaw, double width, double runtime, const std::string& outputFile)
	{
		// Dubins state space
		ob::StateSpacePtr space(std::make_shared<ob::ReedsSheppStateSpace>());
		double turningRadius = 10.0;
		veh_width = width;
		std::cout << veh_width << std::endl;
		space = std::make_shared<ob::DubinsStateSpace>(turningRadius, false);

		ob::RealVectorBounds bounds(2);
		bounds.setLow(0);
		bounds.setHigh(0, ppm_.getWidth());
		bounds.setHigh(1, ppm_.getHeight());
		space->as<ob::SE2StateSpace>()->setBounds(bounds);

		// define a simple setup class
		ss = std::make_shared<og::SimpleSetup>(space);
		ob::ScopedState<> start(space), goal(space);
		start[0] = start_row;
		start[1] = start_col;
		start[2] = start_yaw;
		goal[0] = goal_row;
		goal[1] = goal_col;
		goal[2] = goal_yaw;
		// set state validity checking for this space
		ss->setStateValidityChecker([this](const ob::State *state) { return isStateValid(state); });

		ss->setStartAndGoalStates(start, goal);
		og::RRT *rrt = new og::RRT(ss->getSpaceInformation());
		rrt->setRange(10.0);
		rrt->setGoalBias(0.05);
		ss->setPlanner(ob::PlannerPtr(rrt));
		// this call is optional, but we put it in to get more output information
		space->setup();
		ss->getSpaceInformation()->setStateValidityCheckingResolution(0.001);

		// attempt to solve the problem within 30 seconds of planning time
		ob::PlannerStatus solved = ss->solve(runtime);

		if (solved)
		{
			std::cout << "Found solution:" << std::endl;
			ss->simplifySolution();
			og::PathGeometric &p = ss->getSolutionPath();
			ss->getPathSimplifier()->simplifyMax(p);
			ss->getPathSimplifier()->smoothBSpline(p);

			//path.printAsMatrix(std::cout);
		}
		else
			std::cout << "No solution found" << std::endl;
	}

	void recordSolution()
	{
		og::PathGeometric &p = ss->getSolutionPath();
		p.interpolate();
		for (std::size_t i = 0; i < p.getStateCount(); ++i)
		{
			const int w = std::min(maxWidth_, (int)p.getState(i)->as<ob::SE2StateSpace::StateType>()->getX());
			const int h = std::min(maxHeight_, (int)p.getState(i)->as<ob::SE2StateSpace::StateType>()->getY());
			std::cout << p.getState(i)->as<ob::SE2StateSpace::StateType>()->getX() << " " << p.getState(i)->as<ob::SE2StateSpace::StateType>()->getY() << std::endl;
			ompl::PPM::Color &c = ppm_.getPixel(h, w);
			c.red = 255;
			c.green = 0;
			c.blue = 0;
		}
	}

	void save(const char *filename)
	{
		ppm_.saveFile(filename);
	}

private:

	bool isStateValid(const ob::State *state) const
	{
		/*const int w1 = std::min((int)state->as<ob::SE2StateSpace::StateType>()->getX(), maxWidth_);
		const int h1 = std::min((int)state->as<ob::SE2StateSpace::StateType>()->getY(), maxHeight_);*/
		const double x = state->as<ob::SE2StateSpace::StateType>()->getX();
		const double y = state->as<ob::SE2StateSpace::StateType>()->getY();
		const double yaw = state->as<ob::SE2StateSpace::StateType>()->getYaw();
		const double left_x = x + veh_width * sin(yaw);
		const double left_y = y - veh_width * cos(yaw);
		const double right_x = x - veh_width * sin(yaw);
		const double right_y = y + veh_width * cos(yaw);
		const ompl::PPM::Color &left_c = ppm_.getPixel(round(left_y), round(left_x));
		const ompl::PPM::Color &right_c = ppm_.getPixel(round(right_y), round(right_x));
		return left_c.red > 127 && left_c.green > 127 && left_c.blue > 127 && right_c.red > 127 && right_c.green > 127 && right_c.blue > 127;
	}

	og::SimpleSetupPtr ss;
	int maxWidth_;
	int maxHeight_;
	ompl::PPM ppm_;
	double veh_width;

};

int main()
{
	int length = 20;
	for (int i = 0; i < length; i++)
	{
		Plane2DEnvironment env("C:/Users/TJZXH/Desktop/test.ppm");
		double runTime = 100.0;
		const std::string& outputFile = "J:/results.txt";
		env.plan(85, 626, -3.14/4, 521, 12, -3.14/2, 10.0, runTime, outputFile);
		env.recordSolution();
		std::string env_path = "C:/Users/TJZXH/Desktop/OMPL-LEARN/veh_width_results/veh_width_demo" + std::to_string(i) + ".ppm";
		env.save(env_path.c_str());
	}
	return 0;
}


#ifndef __DYNAUTODIFF_EIGENHELPER__
#define __DYNAUTODIFF_EIGENHELPER__
#include "boost/lexical_cast.hpp"
#include "strtk/strtk.hpp"
#include <boost/json.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/src/Core/util/Constants.h>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <string>
#include <utility>

namespace DynAutoDiff {
// Type definition
template <typename T = double>
using TMat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using TMatd=TMat<double>;

template <typename T = double>
using TArr = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

template <typename T = double> using TVec = Eigen::Matrix<T, Eigen::Dynamic, 1>;
template <typename T = double> using TVecA = Eigen::Array<T, Eigen::Dynamic, 1>;
template <typename T = double> using TRVec = Eigen::Matrix<T, 1, Eigen::Dynamic>;
using TVecd = Eigen::Matrix<double, Eigen::Dynamic, 1>;
template <typename T = double> using TMap = Eigen::Map<TMat<T>>;

// Scalar to a Eigen::Matrix
template <typename T> inline TMat<T> eigen_scalar_mat(const T &v) {
    TMat<T> res(1, 1);
    res << v;
    return res;
};

// Broadcasting mul.
// mxn * 1xn: return mxn, each column of mxn is scaled by each col of 1xn.
// mxn * mx1: likewise.
// Or switching order.
#define BROADCASTINGMULERROR                                                                       \
    throw std::range_error("Invalid broadcasting mul. Shapes are (" + std::to_string(l.rows()) +   \
                           ", " + std::to_string(l.cols()) + "), (" + std::to_string(r.rows()) +   \
                           ", " + std::to_string(r.cols()) + ").");
template <typename T1, typename T2>
TMat<typename T1::Scalar> _broadcasting_mul(const T1 &l, const T2 &r) {
    using T = typename T1::Scalar;
    // mxn, mx1
    if (l.rows() == r.rows()) {
        if (r.cols() == 1) {
            TMat<T> res = l;
            for (int j = 0; j < l.rows(); ++j) {
                res.row(j) = res.row(j) * r.coeff(j, 0);
            }
            return res;
        } else if (l.cols() == 1) {
            return _broadcasting_mul(r, l);
        } else {
            BROADCASTINGMULERROR
        }
    } else if (l.cols() == r.cols()) {
        if (l.rows() == 1) {
            TMat<T> res = r;
            for (int j = 0; j < r.cols(); ++j) {
                res.col(j) = res.col(j) * l.coeff(0, j);
            }
            return res;
        } else if (r.rows() == 1) {
            return _broadcasting_mul(r, l);
        } else {
            BROADCASTINGMULERROR
        }
    } else {
        BROADCASTINGMULERROR
    }
};

// negation operator to a Map<>
/*template<typename T>
TMat<T> operator -(const TMap<T>& op){
        TMat<T> res=op;
        res=-res;
        return res;
};*/

// decide shape of file.
template <typename T = double> auto decide_tab_file_shape(const std::string &filename) {
    std::ifstream file(filename);
    int rows = 0, cols = 0;
    std::string str;
    std::getline(file, str);
    std::stringstream ss(str);
    double tmp;
    while (ss >> tmp) {
        ++cols;
    };
    ++rows;
    while (std::getline(file, str)) {
        if (str.size() > 0)
            ++rows;
    }

    return std::make_pair(rows, cols);
};

// Load tab delimited file. Shape is automatically detected.
template <typename T = double> TMat<T> load_tab_file(const std::string &filename) {
    if (!std::filesystem::exists(std::filesystem::path(filename))) {
        throw std::range_error("File " + filename + " not exists.");
    }
    auto [rows, cols] = decide_tab_file_shape<T>(filename);
    std::ifstream file(filename);
    TMat<T> res(rows, cols);
    for (int i = 0; i < rows; ++i) {
        for (int j = 0; j < cols; ++j) {
            file >> res.coeffRef(i, j);
        }
    }
    return res;
};

// decide shape of csv file.
template <typename T = double> auto decide_csv_file_shape(const std::string &filename) {
    std::ifstream file(filename);
    int rows = 0, cols = 0;
    std::string line;
    std::vector<std::string> line_tokens;
    std::getline(file, line);

    const std::string csv_delimiter = ",";
    strtk::parse(line, csv_delimiter, line_tokens);
    cols = line_tokens.size();

    ++rows;
    while (std::getline(file, line)) {
        if (line.size() > 0)
            ++rows;
    }

    return std::make_pair(rows, cols);
};

// Load csv file. Shape is automatically detected.
template <typename T = double>
TMat<T> load_csv_file(const std::string &filename, int skip_row = 0, int skip_column = 0) {
    auto [rows, cols] = decide_csv_file_shape<T>(filename);
    std::ifstream file(filename);
    TMat<T> res(rows - skip_row, cols - skip_column);

    const std::string csv_delimiter = ",";

    std::string line;

    int k = 0, km = 0;
    while (std::getline(file, line)) {
        std::vector<std::string> line_tokens;
        ++k;
        if (line.size() > 0) {
            strtk::parse(line, csv_delimiter, line_tokens);
            if (line_tokens.size() != cols) {
                throw std::runtime_error("Columns at row " + std::to_string(k) + " is less than " +
                                         std::to_string(cols) + ".");
            };

            if (k <= skip_row) {
                continue;
            } else {
                for (int i = skip_column; i < line_tokens.size(); ++i) {
                    T val;
                    try {
                        val = std::stod(line_tokens[i]);
                    } catch (...) {
                        throw std::runtime_error("Failed to parse number at Row " +
                                                 std::to_string(k) + " Col " + std::to_string(i) +
                                                 ", string is: " + line_tokens[i]);
                    }
                    res.coeffRef(km, i - skip_column) = val;
                };
                ++km;
            };
        };
    }

    return res;
};

template <typename T = double>
TMat<T> load_mat(const std::string &filename, int skip_row = 0, int skip_column = 0) {
    auto p = std::filesystem::path(filename);
    if (!std::filesystem::exists(p)) {
        throw std::range_error("File " + filename + " not exists.");
    };
    // Load data.
    if (p.extension() == std::filesystem::path(".txt")) {
        return load_tab_file<T>(filename);
    } else if (p.extension() == std::filesystem::path(".csv")) {
        return load_csv_file<T>(filename, skip_row, skip_column);
    } else {
        throw std::runtime_error("Unknown file type " + p.extension().string());
    }
};

template <typename T>
void write_mat(const T &mat, const std::string &filename, const std::string &delimiter = ",") {
    std::ofstream file(filename);
    int m = mat.rows(), n = mat.cols();
    for (int i = 0; i < m - 1; ++i) {
        for (int j = 0; j < n - 1; ++j) {
            file << mat.coeff(i, j) << delimiter;
        }
        file << mat.coeff(i, n - 1) << std::endl;
    }
    for (int j = 0; j < n - 1; ++j) {
        file << mat.coeff(m - 1, j) << delimiter;
    }
    file << mat.coeff(m - 1, n - 1);
}
// Convert a matrix to a json array.
template <typename T> boost::json::array to_json_array(const T &data) {
    boost::json::array res;
    for (int i = 0; i < data.rows(); ++i) {
        boost::json::array arr;
        for (int j = 0; j < data.cols(); ++j) {
            arr.emplace_back(data.coeff(i, j));
        }
        res.emplace_back(arr);
    }
    return res;
};
template <std::floating_point T = double> TMat<T> from_json_array(const boost::json::array &data) {
    int rows = data.size(), cols = data[0].as_array().size();
    TMat<T> res(rows, cols);
    for (int i = 0; i < rows; ++i) {
        for (int j = 0; j < cols; ++j) {
            res.coeffRef(i, j) = data[i].as_array()[j].get_double();
        }
    }
    return res;
};
template <typename T = double> inline T sign(T x) {
    if (x > 0)
        return 1;
    else if (x == 0)
        return 0;
    else
        return -1;
};

}; // namespace DynAutoDiff
#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <sstream>

#include "main.h"

#include <Eigen/src/Core/arch/CUDA/Half.h>

#ifdef EIGEN_HAS_CUDA_FP16
#error "EIGEN_HAS_CUDA_FP16 should not be defined in this CPU unit test"
#endif

// Make sure it's possible to forward declare Eigen::half
namespace Eigen {
struct half;
}

using Eigen::half;

void test_conversion()
{
  using Eigen::half_impl::__half_raw;

  // Conversion from float.
  VERIFY_IS_EQUAL(half(1.0f).x, 0x3c00);
  VERIFY_IS_EQUAL(half(0.5f).x, 0x3800);
  VERIFY_IS_EQUAL(half(0.33333f).x, 0x3555);
  VERIFY_IS_EQUAL(half(0.0f).x, 0x0000);
  VERIFY_IS_EQUAL(half(-0.0f).x, 0x8000);
  VERIFY_IS_EQUAL(half(65504.0f).x, 0x7bff);
  VERIFY_IS_EQUAL(half(65536.0f).x, 0x7c00);  // Becomes infinity.

  // Denormals.
  VERIFY_IS_EQUAL(half(-5.96046e-08f).x, 0x8001);
  VERIFY_IS_EQUAL(half(5.96046e-08f).x, 0x0001);
  VERIFY_IS_EQUAL(half(1.19209e-07f).x, 0x0002);

  // Verify round-to-nearest-even behavior.
  float val1 = float(half(__half_raw(0x3c00)));
  float val2 = float(half(__half_raw(0x3c01)));
  float val3 = float(half(__half_raw(0x3c02)));
  VERIFY_IS_EQUAL(half(0.5f * (val1 + val2)).x, 0x3c00);
  VERIFY_IS_EQUAL(half(0.5f * (val2 + val3)).x, 0x3c02);

  // Conversion from int.
  VERIFY_IS_EQUAL(half(-1).x, 0xbc00);
  VERIFY_IS_EQUAL(half(0).x, 0x0000);
  VERIFY_IS_EQUAL(half(1).x, 0x3c00);
  VERIFY_IS_EQUAL(half(2).x, 0x4000);
  VERIFY_IS_EQUAL(half(3).x, 0x4200);

  // Conversion from bool.
  VERIFY_IS_EQUAL(half(false).x, 0x0000);
  VERIFY_IS_EQUAL(half(true).x, 0x3c00);

  // Conversion to float.
  VERIFY_IS_EQUAL(float(half(__half_raw(0x0000))), 0.0f);
  VERIFY_IS_EQUAL(float(half(__half_raw(0x3c00))), 1.0f);

  // Denormals.
  VERIFY_IS_APPROX(float(half(__half_raw(0x8001))), -5.96046e-08f);
  VERIFY_IS_APPROX(float(half(__half_raw(0x0001))), 5.96046e-08f);
  VERIFY_IS_APPROX(float(half(__half_raw(0x0002))), 1.19209e-07f);

  // NaNs and infinities.
  VERIFY(!(numext::isinf)(float(half(65504.0f))));  // Largest finite number.
  VERIFY(!(numext::isnan)(float(half(0.0f))));
  VERIFY((numext::isinf)(float(half(__half_raw(0xfc00)))));
  VERIFY((numext::isnan)(float(half(__half_raw(0xfc01)))));
  VERIFY((numext::isinf)(float(half(__half_raw(0x7c00)))));
  VERIFY((numext::isnan)(float(half(__half_raw(0x7c01)))));

#if !EIGEN_COMP_MSVC
  // Visual Studio errors out on divisions by 0
  VERIFY((numext::isnan)(float(half(0.0 / 0.0))));
  VERIFY((numext::isinf)(float(half(1.0 / 0.0))));
  VERIFY((numext::isinf)(float(half(-1.0 / 0.0))));
#endif

  // Exactly same checks as above, just directly on the half representation.
  VERIFY(!(numext::isinf)(half(__half_raw(0x7bff))));
  VERIFY(!(numext::isnan)(half(__half_raw(0x0000))));
  VERIFY((numext::isinf)(half(__half_raw(0xfc00))));
  VERIFY((numext::isnan)(half(__half_raw(0xfc01))));
  VERIFY((numext::isinf)(half(__half_raw(0x7c00))));
  VERIFY((numext::isnan)(half(__half_raw(0x7c01))));

#if !EIGEN_COMP_MSVC
  // Visual Studio errors out on divisions by 0
  VERIFY((numext::isnan)(half(0.0 / 0.0)));
  VERIFY((numext::isinf)(half(1.0 / 0.0)));
  VERIFY((numext::isinf)(half(-1.0 / 0.0)));
#endif
}

void test_numtraits()
{
  std::cout << "epsilon       = " << NumTraits<half>::epsilon() << "  (0x" << std::hex << NumTraits<half>::epsilon().x << ")" << std::endl;
  std::cout << "highest       = " << NumTraits<half>::highest() << "  (0x" << std::hex << NumTraits<half>::highest().x << ")" << std::endl;
  std::cout << "lowest        = " << NumTraits<half>::lowest() << "  (0x" << std::hex << NumTraits<half>::lowest().x << ")" << std::endl;
  std::cout << "min           = " << (std::numeric_limits<half>::min)() << "  (0x" << std::hex << half((std::numeric_limits<half>::min)()).x << ")" << std::endl;
  std::cout << "denorm min    = " << (std::numeric_limits<half>::denorm_min)() << "  (0x" << std::hex << half((std::numeric_limits<half>::denorm_min)()).x << ")" << std::endl;
  std::cout << "infinity      = " << NumTraits<half>::infinity() << "  (0x" << std::hex << NumTraits<half>::infinity().x << ")" << std::endl;
  std::cout << "quiet nan     = " << NumTraits<half>::quiet_NaN() << "  (0x" << std::hex << NumTraits<half>::quiet_NaN().x << ")" << std::endl;
  std::cout << "signaling nan = " << std::numeric_limits<half>::signaling_NaN() << "  (0x" << std::hex << std::numeric_limits<half>::signaling_NaN().x << ")" << std::endl;

  VERIFY(NumTraits<half>::IsSigned);

  VERIFY_IS_EQUAL( std::numeric_limits<half>::infinity().x, half(std::numeric_limits<float>::infinity()).x );
  VERIFY_IS_EQUAL( std::numeric_limits<half>::quiet_NaN().x, half(std::numeric_limits<float>::quiet_NaN()).x );
  VERIFY_IS_EQUAL( std::numeric_limits<half>::signaling_NaN().x, half(std::numeric_limits<float>::signaling_NaN()).x );
  VERIFY( (std::numeric_limits<half>::min)() > half(0.f) );
  VERIFY( (std::numeric_limits<half>::denorm_min)() > half(0.f) );
  VERIFY( (std::numeric_limits<half>::min)()/half(2) > half(0.f) );
  VERIFY_IS_EQUAL( (std::numeric_limits<half>::denorm_min)()/half(2), half(0.f) );
}

void test_arithmetic()
{
  VERIFY_IS_EQUAL(float(half(2) + half(2)), 4);
  VERIFY_IS_EQUAL(float(half(2) + half(-2)), 0);
  VERIFY_IS_APPROX(float(half(0.33333f) + half(0.66667f)), 1.0f);
  VERIFY_IS_EQUAL(float(half(2.0f) * half(-5.5f)), -11.0f);
  VERIFY_IS_APPROX(float(half(1.0f) / half(3.0f)), 0.33333f);
  VERIFY_IS_EQUAL(float(-half(4096.0f)), -4096.0f);
  VERIFY_IS_EQUAL(float(-half(-4096.0f)), 4096.0f);
}

void test_comparison()
{
  VERIFY(half(1.0f) > half(0.5f));
  VERIFY(half(0.5f) < half(1.0f));
  VERIFY(!(half(1.0f) < half(0.5f)));
  VERIFY(!(half(0.5f) > half(1.0f)));

  VERIFY(!(half(4.0f) > half(4.0f)));
  VERIFY(!(half(4.0f) < half(4.0f)));

  VERIFY(!(half(0.0f) < half(-0.0f)));
  VERIFY(!(half(-0.0f) < half(0.0f)));
  VERIFY(!(half(0.0f) > half(-0.0f)));
  VERIFY(!(half(-0.0f) > half(0.0f)));

  VERIFY(half(0.2f) > half(-1.0f));
  VERIFY(half(-1.0f) < half(0.2f));
  VERIFY(half(-16.0f) < half(-15.0f));

  VERIFY(half(1.0f) == half(1.0f));
  VERIFY(half(1.0f) != half(2.0f));

  // Comparisons with NaNs and infinities.
#if !EIGEN_COMP_MSVC
  // Visual Studio errors out on divisions by 0
  VERIFY(!(half(0.0 / 0.0) == half(0.0 / 0.0)));
  VERIFY(half(0.0 / 0.0) != half(0.0 / 0.0));

  VERIFY(!(half(1.0) == half(0.0 / 0.0)));
  VERIFY(!(half(1.0) < half(0.0 / 0.0)));
  VERIFY(!(half(1.0) > half(0.0 / 0.0)));
  VERIFY(half(1.0) != half(0.0 / 0.0));

  VERIFY(half(1.0) < half(1.0 / 0.0));
  VERIFY(half(1.0) > half(-1.0 / 0.0));
#endif
}

void test_basic_functions()
{
  VERIFY_IS_EQUAL(float(numext::abs(half(3.5f))), 3.5f);
  VERIFY_IS_EQUAL(float(abs(half(3.5f))), 3.5f);
  VERIFY_IS_EQUAL(float(numext::abs(half(-3.5f))), 3.5f);
  VERIFY_IS_EQUAL(float(abs(half(-3.5f))), 3.5f);

  VERIFY_IS_EQUAL(float(numext::floor(half(3.5f))), 3.0f);
  VERIFY_IS_EQUAL(float(floor(half(3.5f))), 3.0f);
  VERIFY_IS_EQUAL(float(numext::floor(half(-3.5f))), -4.0f);
  VERIFY_IS_EQUAL(float(floor(half(-3.5f))), -4.0f);

  VERIFY_IS_EQUAL(float(numext::ceil(half(3.5f))), 4.0f);
  VERIFY_IS_EQUAL(float(ceil(half(3.5f))), 4.0f);
  VERIFY_IS_EQUAL(float(numext::ceil(half(-3.5f))), -3.0f);
  VERIFY_IS_EQUAL(float(ceil(half(-3.5f))), -3.0f);

  VERIFY_IS_APPROX(float(numext::sqrt(half(0.0f))), 0.0f);
  VERIFY_IS_APPROX(float(sqrt(half(0.0f))), 0.0f);
  VERIFY_IS_APPROX(float(numext::sqrt(half(4.0f))), 2.0f);
  VERIFY_IS_APPROX(float(sqrt(half(4.0f))), 2.0f);

  VERIFY_IS_APPROX(float(numext::pow(half(0.0f), half(1.0f))), 0.0f);
  VERIFY_IS_APPROX(float(pow(half(0.0f), half(1.0f))), 0.0f);
  VERIFY_IS_APPROX(float(numext::pow(half(2.0f), half(2.0f))), 4.0f);
  VERIFY_IS_APPROX(float(pow(half(2.0f), half(2.0f))), 4.0f);

  VERIFY_IS_EQUAL(float(numext::exp(half(0.0f))), 1.0f);
  VERIFY_IS_EQUAL(float(exp(half(0.0f))), 1.0f);
  VERIFY_IS_APPROX(float(numext::exp(half(EIGEN_PI))), 20.f + float(EIGEN_PI));
  VERIFY_IS_APPROX(float(exp(half(EIGEN_PI))), 20.f + float(EIGEN_PI));

  VERIFY_IS_EQUAL(float(numext::log(half(1.0f))), 0.0f);
  VERIFY_IS_EQUAL(float(log(half(1.0f))), 0.0f);
  VERIFY_IS_APPROX(float(numext::log(half(10.0f))), 2.30273f);
  VERIFY_IS_APPROX(float(log(half(10.0f))), 2.30273f);

  VERIFY_IS_EQUAL(float(numext::log1p(half(0.0f))), 0.0f);
  VERIFY_IS_EQUAL(float(log1p(half(0.0f))), 0.0f);
  VERIFY_IS_APPROX(float(numext::log1p(half(10.0f))), 2.3978953f);
  VERIFY_IS_APPROX(float(log1p(half(10.0f))), 2.3978953f);
}

void test_trigonometric_functions()
{
  VERIFY_IS_APPROX(numext::cos(half(0.0f)), half(cosf(0.0f)));
  VERIFY_IS_APPROX(cos(half(0.0f)), half(cosf(0.0f)));
  VERIFY_IS_APPROX(numext::cos(half(EIGEN_PI)), half(cosf(EIGEN_PI)));
  //VERIFY_IS_APPROX(numext::cos(half(EIGEN_PI/2)), half(cosf(EIGEN_PI/2)));
  //VERIFY_IS_APPROX(numext::cos(half(3*EIGEN_PI/2)), half(cosf(3*EIGEN_PI/2)));
  VERIFY_IS_APPROX(numext::cos(half(3.5f)), half(cosf(3.5f)));

  VERIFY_IS_APPROX(numext::sin(half(0.0f)), half(sinf(0.0f)));
  VERIFY_IS_APPROX(sin(half(0.0f)), half(sinf(0.0f)));
  //  VERIFY_IS_APPROX(numext::sin(half(EIGEN_PI)), half(sinf(EIGEN_PI)));
  VERIFY_IS_APPROX(numext::sin(half(EIGEN_PI/2)), half(sinf(EIGEN_PI/2)));
  VERIFY_IS_APPROX(numext::sin(half(3*EIGEN_PI/2)), half(sinf(3*EIGEN_PI/2)));
  VERIFY_IS_APPROX(numext::sin(half(3.5f)), half(sinf(3.5f)));

  VERIFY_IS_APPROX(numext::tan(half(0.0f)), half(tanf(0.0f)));
  VERIFY_IS_APPROX(tan(half(0.0f)), half(tanf(0.0f)));
  //  VERIFY_IS_APPROX(numext::tan(half(EIGEN_PI)), half(tanf(EIGEN_PI)));
  //  VERIFY_IS_APPROX(numext::tan(half(EIGEN_PI/2)), half(tanf(EIGEN_PI/2)));
  //VERIFY_IS_APPROX(numext::tan(half(3*EIGEN_PI/2)), half(tanf(3*EIGEN_PI/2)));
  VERIFY_IS_APPROX(numext::tan(half(3.5f)), half(tanf(3.5f)));
}

void test_array()
{
  typedef Array<half,1,Dynamic> ArrayXh;
  Index size = internal::random<Index>(1,10);
  Index i = internal::random<Index>(0,size-1);
  ArrayXh a1 = ArrayXh::Random(size), a2 = ArrayXh::Random(size);
  VERIFY_IS_APPROX( a1+a1, half(2)*a1 );
  VERIFY( (a1.abs() >= half(0)).all() );
  VERIFY_IS_APPROX( (a1*a1).sqrt(), a1.abs() );

  VERIFY( ((a1.min)(a2) <= (a1.max)(a2)).all() );
  a1(i) = half(-10.);
  VERIFY_IS_EQUAL( a1.minCoeff(), half(-10.) );
  a1(i) = half(10.);
  VERIFY_IS_EQUAL( a1.maxCoeff(), half(10.) );

  std::stringstream ss;
  ss << a1;
}

void test_half_float()
{
  CALL_SUBTEST(test_conversion());
  CALL_SUBTEST(test_numtraits());
  CALL_SUBTEST(test_arithmetic());
  CALL_SUBTEST(test_comparison());
  CALL_SUBTEST(test_basic_functions());
  CALL_SUBTEST(test_trigonometric_functions());
  CALL_SUBTEST(test_array());
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::response::right_truncated::mean_event.hpp   		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_MEAN_EVENT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_MEAN_EVENT_HPP_ER_2009
#include <stdexcept>
#include <boost/type_traits.hpp>
#include <boost/range.hpp>
#include <boost/format.hpp>
#include <boost/operators.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace response{

    // This class can be used to accumulate the proportion of failures and
    // mean event time in a sequence of events.
    //
    // In the case of the exponential, a collection of mean_events can
    // form a sufficient statistic,  assuming a discretized domain for
    // the covariate: domain(x) = {x[k]:k=0,...,K-1}
    //
    // $\log L_i(\beta) = \nu_i \eta_i - t_i \exp(\eta_i)$
    // where $\eta_i = \langle x_i,beta \rangle$
    // log L(D,\beta) = sum{
    //    <x[k],\beta> sum{\nu[i]:x[i]==x[k]}
    //     -exp(<x[k],\beta>) sum{t[i] : x[i]==x[k]}
    //  :k=1,...,K
    // }
    // Sufficient statistic:
    // {(sum{\nu[i]:x[i]==x[k]},sum{t[i] : x[i]==x[k]}):k=0,...,K-1}
    template<typename T,typename B = bool>
    class mean_event :
        public event<T,T>,
        boost::addable<
            mean_event<T,B>
        >
        // TODO equality_comparable<mean_event<T,B> >
    {
    public:
        typedef event<T,B>  event_;
        typedef event<T,T>  super_;
        typedef typename super_::value_type value_type;
        typedef std::size_t                 size_type;

        // Construction
        mean_event();
        mean_event(const event_&);
        mean_event(const mean_event&);
        mean_event& operator=(const mean_event&);

        // Access
        size_type count()const;

        // Operators
        mean_event& operator+=( const mean_event& e );
        mean_event& operator()( const event_& e );

        // I/O
        template<class Archive>
        void serialize(Archive & ar, const unsigned int version);

        private:
        value_type impl(size_type n_a,value_type a,size_type n_b,value_type b);
        size_type  count_;
        static super_ convert(const event_&);
    };

    template<typename T,typename B>
    std::ostream& operator<<(std::ostream& out,const mean_event<T,B>& e);

    template<typename E>
    struct meta_mean_event{
        typedef typename E::value_type value_type;
        typedef mean_event<value_type> type;
    };

    // Implementation //

    // Constructor
    template<typename T,typename B>
    mean_event<T,B>::mean_event()
    :super_(static_cast<T>(false),static_cast<T>(0)),
    count_(0){
        // Using super_() instead, would initialize time to inf.
    }

    template<typename T,typename B>
    mean_event<T,B>::mean_event(const event_& e)
    :super_(convert(e)),count_(1){
        const char* msg = "mean_event::mean_event(e), isinf(e.time())";
        if(boost::math::isinf(this->time())){
            throw std::runtime_error(
                msg
            );
        }
    }

    template<typename T,typename B>
    mean_event<T,B>::mean_event(const mean_event& that)
    :super_(that),count_(that.count_){}

    template<typename T,typename B>
    mean_event<T,B>&
    mean_event<T,B>::operator=(const mean_event& that){
        if(&that!=this){
            super_::operator=( that );
            count_ = that.count_;
        }
        return *this;
    }

    // Access
    template<typename T,typename B>
    typename mean_event<T,B>::size_type
    mean_event<T,B>::count()const{ return count_; }

    // Update
    template<typename T,typename B>
    mean_event<T,B>&
    mean_event<T,B>::operator()(const event_& e){
        mean_event other(e);
        return ( (*this) += other );
    }

    template<typename T,typename B>
    mean_event<T,B>&
    mean_event<T,B>::operator+=(const mean_event& other){
        size_type n_a = this->count();
        size_type n_b = other.count();
        (this->failure_)
            = (this->impl(n_a,this->failure(),n_b,other.failure()));
        (this->time_)
            = (this->impl(n_a,this->time(),n_b,other.time()));
        (this->count_) += n_b;
        return *this;
    }

    //Private
    template<typename T,typename B>
    template<class Archive>
    void mean_event<T,B>::serialize(Archive & ar, const unsigned int version)
    {
        ar & boost::serialization::base_object<super_>(*this);
        ar & count_;
    }

    template<typename T,typename B>
    typename mean_event<T,B>::value_type
    mean_event<T,B>::impl(
            size_type n_a,
            value_type a,
            size_type n_b,
            value_type b
    ){
        return ( (n_a * a) + (n_b * b) ) / (n_a + n_b);
    }

    template<typename T,typename B>
    std::ostream& operator<<(std::ostream& out,const mean_event<T,B>& that){
        typedef mean_event<T,B> that_;
        typedef typename that_::super_ super_;
        static const char* str = "(%1%,%2%)";
        format f(str);
        f % that.count() % static_cast<const super_&>(that) ;
        out << f.str();
        return out;
    }

    template<typename T,typename B>
    typename mean_event<T,B>::super_
    mean_event<T,B>::convert(const event_& e){
        static value_type zero = static_cast<value_type>(0);
        static value_type one = static_cast<value_type>(1);
        value_type t = e.time();
        if(e.failure()){
            return super_( one, t );
        }else{
            return super_( zero, t );
        }
    }

}// response
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


/*
 * MIT License
 *
 * Copyright (c) 2019 Camille Schreck
 *
 * 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.
 *
 *----------------------------------------------------------------------------
 * Solve FS interactive (aperiodic) SOP
 *----------------------------------------------------------------------------
 * Compute amplitude of the obstacle sources (input 0) such that the boundary condition are
 * respected (as well as possible, least square) at the boundary points (input 1) given the
 * incoming waves (input 2).
 */

#ifndef __SOP_Solve_FS_inter_h__
#define __SOP_Solve_FS_inter_h__

#include <SOP/SOP_Node.h>
#include "definitions.hpp"
#include <Eigen/SVD>

class SOP_Solve_FS_inter : public SOP_Node {
public:
    SOP_Solve_FS_inter(OP_Network *net, const char *name, OP_Operator *op);
    virtual ~SOP_Solve_FS_inter();

    static PRM_Template myTemplateList[];
    static OP_Node *myConstructor(OP_Network*, const char *, OP_Operator *);
    virtual OP_ERROR             cookInputGroups(OP_Context &context,
                                                int alone = 0);

protected:
    virtual OP_ERROR cookMySop(OP_Context &context);
private:
    void        getGroups(UT_String &str){ evalString(str, "group", 0, 0); }

    const GA_PointGroup *myGroup;

  std::vector<float> wave_lengths;
  std::vector<int> ampli_steps;
  std::vector<VectorXcf> p_in;
  std::vector<Eigen::BDCSVD<MatrixXcf> > svd;
  int buffer_size;
  float damping_coef;
};

#endif


/***************************************************************************
 * Copyright 1998-2020 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * 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 <map>
#include <vector>
#include <string>
#include <queue>
#include <limits>

#include <boost/format.hpp>

#include "luxrays/core/exttrianglemesh.h"
#include "slg/shapes/simplify.h"
#include "slg/scene/scene.h"
#include "slg/utils/harlequincolors.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
//
// The following code is based on Sven Forstmann's quadric mesh simplification
// code (https://github.com/sp4cerat/Fast-Quadric-Mesh-Simplification)
// and heavily modified for LuxCoreRender
//
// Papers at https://mgarland.org/research/quadrics.html

/////////////////////////////////////////////
//
// Mesh Simplification Tutorial
//
// (C) by Sven Forstmann in 2014
//
// License : MIT
// http://opensource.org/licenses/MIT
//
// https://github.com/sp4cerat/Fast-Quadric-Mesh-Simplification
//
// 5/2016: Chris Rorden created minimal version for OSX/Linux/Windows compile

class SymetricMatrix {
public:
	// Constructor
	SymetricMatrix(const float c = 0.f) {
		for (u_int i = 0; i < 10; ++i)
			m[i] = c;
	}

	SymetricMatrix(
			const float m11, const float m12, const float m13, const float m14,
			const float m22, const float m23, const float m24,
			const float m33, const float m34,
			const float m44) {
		m[0] = m11;
		m[1] = m12;
		m[2] = m13;
		m[3] = m14;
		m[4] = m22;
		m[5] = m23;
		m[6] = m24;
		m[7] = m33;
		m[8] = m34;
		m[9] = m44;
	}

	// Make plane
	SymetricMatrix(const float a, const float b, const float c, const float d) {
		m[0] = a * a;
		m[1] = a * b;
		m[2] = a * c;
		m[3] = a * d;
		m[4] = b * b;
		m[5] = b * c;
		m[6] = b * d;
		m[7] = c * c;
		m[8] = c * d;
		m[9] = d * d;
	}

	float operator[](int c) const {
		return m[c];
	}

	// Determinant
	float det(
			const u_int a11, const u_int a12, const u_int a13,
			const u_int a21, const u_int a22, const u_int a23,
			const u_int a31, const u_int a32, const u_int a33) const {
		const float det = m[a11] * m[a22] * m[a33] + m[a13] * m[a21] * m[a32] + m[a12] * m[a23] * m[a31]
				- m[a13] * m[a22] * m[a31] - m[a11] * m[a23] * m[a32] - m[a12] * m[a21] * m[a33];
		return det;
	}

	const SymetricMatrix operator+(const SymetricMatrix &n) const {
		return SymetricMatrix(
				m[0] + n[0], m[1] + n[1], m[2] + n[2], m[3] + n[3],
				m[4] + n[4], m[5] + n[5], m[6] + n[6],
				m[7] + n[7], m[8] + n[8],
				m[9] + n[9]);
	}

	SymetricMatrix& operator+=(const SymetricMatrix& n) {
		m[0] += n[0];
		m[1] += n[1];
		m[2] += n[2];
		m[3] += n[3];
		m[4] += n[4];
		m[5] += n[5];
		m[6] += n[6];
		m[7] += n[7];
		m[8] += n[8];
		m[9] += n[9];

		return *this;
	}

	float m[10];
};

class Simplify {
public:
	Simplify(const ExtTriangleMesh &srcMesh) {
		const u_int vertCount = srcMesh.GetTotalVertexCount();
		const u_int triCount = srcMesh.GetTotalTriangleCount();
		const Point *verts = srcMesh.GetVertices();
		const Triangle *tris = srcMesh.GetTriangles();

		vertices.resize(vertCount);
		for (u_int i = 0; i < vertCount; ++i)
			vertices[i].p = verts[i];

		if (srcMesh.HasNormals()) {
			const Normal *norms = srcMesh.GetNormals();
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].norm = norms[i];

			hasNormals = true;
		} else
			hasNormals = false;

		if (srcMesh.HasUVs(0)) {
			const UV *uvs = srcMesh.GetUVs(0);
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].uv = uvs[i];

			hasUVs = true;
		} else
			hasUVs = false;

		if (srcMesh.HasColors(0)) {
			const Spectrum *cols = srcMesh.GetColors(0);
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].col = cols[i];

			hasColors = true;
		} else
			hasColors = false;

		if (srcMesh.HasAlphas(0)) {
			const float *alphas = srcMesh.GetAlphas(0);
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].alpha = alphas[i];

			hasAlphas = true;
		} else
			hasAlphas = false;

		triangles.resize(triCount);
		for (u_int i = 0; i < triCount; ++i) {
			triangles[i].v[0] = tris[i].v[0];
			triangles[i].v[1] = tris[i].v[1];
			triangles[i].v[2] = tris[i].v[2];
		}
	}

	~Simplify() {
	}

	ExtTriangleMesh *GetExtMesh() const {
		const u_int vertCount = vertices.size();
		const u_int triCount = triangles.size();

		Point *newVertices = ExtTriangleMesh::AllocVerticesBuffer(vertCount);
		for (u_int i = 0; i < vertCount; ++i)
			newVertices[i] = vertices[i].p;

		Normal *newNorms = nullptr;
		if (hasNormals) {
			newNorms = new Normal[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newNorms[i] = vertices[i].norm;
		}

		UV *newUVs = nullptr;
		if (hasUVs) {
			newUVs = new UV[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newUVs[i] = vertices[i].uv;
		}

		Spectrum *newCols = nullptr;
		if (hasColors) {
			newCols = new Spectrum[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newCols[i] = vertices[i].col;
		}

		float *newAlphas = nullptr;
		if (hasAlphas) {
			newAlphas = new float[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newAlphas[i] = vertices[i].alpha;
		}

		Triangle *newTris = ExtTriangleMesh::AllocTrianglesBuffer(triCount);
		for (u_int i = 0; i < triCount; ++i) {
			assert (triangles[i].v[0] < vertCount);
			newTris[i].v[0] = triangles[i].v[0];

			assert (triangles[i].v[1] < vertCount);
			newTris[i].v[1] = triangles[i].v[1];

			assert (triangles[i].v[2] < vertCount);
			newTris[i].v[2] = triangles[i].v[2];
		}

		return new ExtTriangleMesh(vertCount, triCount, newVertices, newTris, newNorms,
				newUVs, newCols, newAlphas);
	}

	void Decimate(const float targetTriangleCount, const Camera *scnCamera,
			const float screenSize, const bool border) {
		preserveBorder = border;
		camera = scnCamera;
		edgeScreenSize = screenSize;

		// Work on 10% of all triangles for each iteration
		maxCandidateQueueSize = Max(64u, Floor2UInt(triangles.size() * .1f));

		// Init
		for (u_int i = 0; i < triangles.size(); ++i)
			triangles[i].deleted = false;

		// Main iteration loop
		const u_int startTriangleCount = triangles.size();
		deletedTriangles = 0;
		vector<bool> deleted0, deleted1;
		for (u_int iteration = 0; iteration < 64; ++iteration) {
			if (startTriangleCount - deletedTriangles <= targetTriangleCount)
				break;

			const u_int initialdeletedTriangles = deletedTriangles;

			// Update mesh constantly
			UpdateMesh(iteration);

			// Remove vertices & mark deleted triangles
			for (u_int i = 0; i < candidateList.size(); ++i)
				CollapseEdge(candidateList[i].tid, candidateList[i].tvertex, deleted0, deleted1);

			const u_int iterationDeletedTriangles = deletedTriangles - initialdeletedTriangles;
			SDL_LOG("Simplify iteration " << iteration << " (" << candidateList.size() << " edge candidates, deleted " << iterationDeletedTriangles << "/" << deletedTriangles << " of " << startTriangleCount << " triangles)");
			if (iterationDeletedTriangles == 0)
				break;
		}

		// Clean up mesh
		CompactMesh();
	}

private:
	struct SimplifyTriangle {
		u_int v[3];
		Normal geometryN;
		float err[3];
		bool deleted, dirty;
	};

	struct SimplifyVertex {
		Point p;
		Normal norm;
		UV uv;
		Spectrum col;
		float alpha;

		u_int tstart, tcount;
		SymetricMatrix q;

		bool border;
	};

	struct SimplifyRef {
		u_int tid, tvertex;
	};

	class SimplifyRefErrCompare {
	public:
		SimplifyRefErrCompare(const Simplify &s) : simplify(s) { }

		bool operator()(const SimplifyRef &sr1, const SimplifyRef &sr2) const {
			return simplify.triangles[sr1.tid].err[sr1.tvertex] < simplify.triangles[sr2.tid].err[sr2.tvertex];
		}

	private:
		const Simplify &simplify;
	};

	vector<SimplifyTriangle> triangles;
	vector<SimplifyVertex> vertices;
	vector<SimplifyRef> refs;

	const Camera *camera;
	float edgeScreenSize;

	u_int maxCandidateQueueSize;
	vector<SimplifyRef> candidateList;

	u_int deletedTriangles;
	bool hasNormals, hasUVs, hasColors, hasAlphas, preserveBorder;

	bool CollapseEdge(const u_int trinagleIndex, const u_int startVertexIndex,
			vector<bool> &deleted0, vector<bool> &deleted1) {
		SimplifyTriangle &t = triangles[trinagleIndex];

		if (t.deleted)
			return false;
		if (t.dirty)
			return false;

		const u_int i0 = t.v[startVertexIndex];
		SimplifyVertex &v0 = vertices[i0];

		const u_int i1 = t.v[(startVertexIndex + 1) % 3];
		SimplifyVertex &v1 = vertices[i1];

		// Border check
		if (v0.border != v1.border)
			return false;

		// Compute vertex to collapse to
		Point p;
		CalculateCollapseError(i0, i1, &p);

		// true/false if the triangles referencing the vertex are deleted
		deleted0.resize(v0.tcount);
		deleted1.resize(v1.tcount);

		// Don't remove if flipped
		if (Flipped(p, i0, i1, &deleted0))
			return false;
		if (Flipped(p, i1, i0, &deleted1))
			return false;

		// Save original vertex information
		const Point triPoint0 = vertices[t.v[0]].p;
		const Point triPoint1 = vertices[t.v[1]].p;
		const Point triPoint2 = vertices[t.v[2]].p;

		const Normal triNorm0 = vertices[t.v[0]].norm;
		const Normal triNorm1 = vertices[t.v[1]].norm;
		const Normal triNorm2 = vertices[t.v[2]].norm;

		const UV triUV0 = vertices[t.v[0]].uv;
		const UV triUV1 = vertices[t.v[1]].uv;
		const UV triUV2 = vertices[t.v[2]].uv;

		const Spectrum triCol0 = vertices[t.v[0]].col;
		const Spectrum triCol1 = vertices[t.v[1]].col;
		const Spectrum triCol2 = vertices[t.v[2]].col;

		const float triAlpha0 = vertices[t.v[0]].alpha;
		const float triAlpha1 = vertices[t.v[1]].alpha;
		const float triAlpha2 = vertices[t.v[2]].alpha;

		// Not flipped, so remove edge
		v0.p = p;
		v0.q = v1.q + v0.q;

		// Interpolate other vertex attributes
		float b1, b2;
		if (Triangle::GetBaryCoords(
				triPoint0,
				triPoint1,
				triPoint2,
				p, &b1, &b2)) {
			const float b0 = 1.f - b1 - b2;

			if (hasNormals)
				v0.norm = Normalize(b0 * triNorm0 + b1 * triNorm1 + b2 * triNorm2);
			if (hasUVs)
				v0.uv = b0 * triUV0 + b1 * triUV1 + b2 * triUV2;
			if (hasColors)
				v0.col = b0 * triCol0 + b1 * triCol1 + b2 * triCol2;
			if (hasAlphas)
				v0.alpha = b0 * triAlpha0 + b1 * triAlpha1 + b2 * triAlpha2;
		} else {
			// Must be a malformed triangle
			if (hasNormals)
				v0.norm = triNorm0;
			if (hasUVs)
				v0.uv = triUV0;
			if (hasColors)
				v0.col = triCol0;
			if (hasAlphas)
				v0.alpha = triAlpha0;
		}

		const u_int tstart = refs.size();

		UpdateTriangles(i0, v0, deleted0);
		UpdateTriangles(i0, v1, deleted1);

		const u_int tcount = refs.size() - tstart;

		if (tcount <= v0.tcount) {
			// Save ram
			if (tcount)
				copy(&refs[tstart], &refs[tstart] + tcount, &refs[v0.tstart]);
		} else
			// Append
			v0.tstart = tstart;

		v0.tcount = tcount;

		return true;
	}

	// Check if a triangle flips when this edge is removed
	bool Flipped(const Point &p, const u_int i0, const u_int i1,
			vector<bool> *deleted = nullptr) const {
		const SimplifyVertex &v0 = vertices[i0];

		for (u_int k = 0; k < v0.tcount; ++k) {
			const SimplifyTriangle &t = triangles[refs[v0.tstart + k].tid];

			if (t.deleted)
				continue;

			const u_int s = refs[v0.tstart + k].tvertex;
			const u_int id1 = t.v[(s + 1) % 3];
			const u_int id2 = t.v[(s + 2) % 3];

			// Delete ?
			if (id1 == i1 || id2 == i1) {
				if (deleted)
					(*deleted)[k] = true;
				continue;
			}

			// Check if the triangle is too narrow
			const Vector d1 = Normalize(vertices[id1].p - p);
			const Vector d2 = Normalize(vertices[id2].p - p);
			if (AbsDot(d1, d2) > .999f)
				return true;

			// Check if the Normal is changing side
			const Normal geometryN(Normalize(Cross(d1, d2)));
			if (Dot(geometryN, t.geometryN) < .2f)
				return true;

			if (deleted)
				(*deleted)[k] = false;
		}

		return false;
	}

	// Update triangle connections and edge error after a edge is collapsed
	void UpdateTriangles(const u_int i0, const SimplifyVertex &v,
			const  vector<bool> &deleted) {
		for (u_int k = 0; k < v.tcount; ++k) {
			const SimplifyRef &r = refs[v.tstart + k];
			SimplifyTriangle &t = triangles[r.tid];

			if (t.deleted)
				continue;

			if (deleted[k]) {
				t.deleted = true;
				deletedTriangles++;
				continue;
			}

			t.v[r.tvertex] = i0;
			t.dirty = true;
			UpdateTriangleError(t);

			refs.push_back(r);
		}
	}

	// Compact triangles, compute edge error and build reference list
	void UpdateMesh(const u_int iteration) {
		if (iteration > 0) {
			// Compact triangles
			int dst = 0;
			for (u_int i = 0; i < triangles.size(); ++i)
				if (!triangles[i].deleted)
					triangles[dst++] = triangles[i];

			triangles.resize(dst);
		}

		// Init Quadrics by Plane & Edge Errors
		//
		// Required at the beginning (iteration == 0)
		//
		if (iteration == 0) {
			for (u_int i = 0; i < vertices.size(); ++i)
				vertices[i].q = SymetricMatrix(0.0);

			for (u_int i = 0; i < triangles.size(); ++i) {
				SimplifyTriangle &t = triangles[i];

				SimplifyVertex &v0 = vertices[t.v[0]];
				SimplifyVertex &v1 = vertices[t.v[1]];
				SimplifyVertex &v2 = vertices[t.v[2]];

				const Normal geometryN(Normalize(Cross(v1.p - v0.p, v2.p - v0.p)));
				t.geometryN = geometryN;

				// It doesn't matter what vertex I use here because the triangle
				// plane will pass for all 3
				const SymetricMatrix sm(geometryN.x, geometryN.y, geometryN.z,
						-Dot(Vector(geometryN), Vector(v0.p)));
				v0.q += sm;
				v1.q += sm;
				v2.q += sm;
			}

			for (u_int i = 0; i < triangles.size(); ++i) {
				// Calc Edge Error
				SimplifyTriangle &t = triangles[i];

				UpdateTriangleError(t);
			}
		}

		// Init Reference ID list
		for (u_int i = 0; i < vertices.size(); ++i) {
			vertices[i].tstart = 0;
			vertices[i].tcount = 0;
		}

		for (u_int i = 0; i < triangles.size(); ++i) {
			SimplifyTriangle &t = triangles[i];

			vertices[t.v[0]].tcount++;
			vertices[t.v[1]].tcount++;
			vertices[t.v[2]].tcount++;
		}

		u_int tstart = 0;
		for (u_int i = 0; i < vertices.size(); ++i) {
			SimplifyVertex &v = vertices[i];

			v.tstart = tstart;
			tstart += v.tcount;
			v.tcount = 0;
		}

		// Write References
		refs.resize(triangles.size() * 3);
		for (u_int i = 0; i < triangles.size(); ++i) {
			SimplifyTriangle &t = triangles[i];

			for (u_int j = 0; j < 3; ++j) {
				SimplifyVertex &v = vertices[t.v[j]];

				refs[v.tstart + v.tcount].tid = i;
				refs[v.tstart + v.tcount].tvertex = j;

				v.tcount++;
			}
		}

		// Identify boundary : vertices[].border=0,1
		//
		// Required at the beginning (iteration == 0)
		if (iteration == 0) {
			for (u_int i = 0; i < vertices.size(); ++i)
				vertices[i].border = false;

			vector<u_int> vcount, vids;
			for (u_int i = 0; i < vertices.size(); ++i) {
				SimplifyVertex &v = vertices[i];
				vcount.clear();
				vids.clear();

				for (u_int j = 0; j < v.tcount; ++j) {
					int k = refs[v.tstart + j].tid;
					SimplifyTriangle &t = triangles[k];

					for (u_int k = 0; k < 3; ++k) {
						u_int ofs = 0;
						u_int id = t.v[k];

						while (ofs < vcount.size()) {
							if (vids[ofs] == id)
								break;

							ofs++;
						}

						if (ofs == vcount.size()) {
							vcount.push_back(1);
							vids.push_back(id);
						} else
							vcount[ofs]++;
					}
				}

				for (u_int j = 0; j < vcount.size(); ++j) {
					if (vcount[j] == 1)
						vertices[vids[j]].border = true;
				}
			}
		}

		// Build the edge candidate queue
		priority_queue<SimplifyRef, vector<SimplifyRef>, SimplifyRefErrCompare>
			candidateQueue{ SimplifyRefErrCompare(*this) };
		for (u_int i = 0; i < triangles.size(); ++i) {
			const SimplifyTriangle &t = triangles[i];

			// Look for the (valid) triangle vertex with the minimum error
			u_int minErrorIndex = NULL_INDEX;
			float minError = numeric_limits<float>::infinity();
			for (u_int j = 0; j < 3; ++j) {
				const u_int i0 = t.v[j];
				SimplifyVertex &v0 = vertices[i0];

				const u_int i1 = t.v[(j + 1) % 3];
				SimplifyVertex &v1 = vertices[i1];

				// Border check
				if (preserveBorder) {
					if (v0.border && v1.border)
						continue;
				} else {
					if (v0.border != v1.border)
						continue;
				}

				// Compute vertex to collapse to
				Point p;
				CalculateCollapseError(i0, i1, &p);

				// Don't remove if flipped
				if (Flipped(p, i0, i1))
					continue;
				if (Flipped(p, i1, i0))
					continue;

				if (t.err[j] < minError) {
					minErrorIndex = j;
					minError = t.err[j];
				}
			}

			if (minErrorIndex == NULL_INDEX)
				continue;

			if (candidateQueue.size() < maxCandidateQueueSize) {
				candidateQueue.push(SimplifyRef{i, minErrorIndex});
				continue;
			}

			const SimplifyRef &top = candidateQueue.top();
			if (t.err[minErrorIndex] < triangles[top.tid].err[top.tvertex]) {
				candidateQueue.pop();
				candidateQueue.push(SimplifyRef{i, minErrorIndex});
			}
		}

		if (candidateQueue.size() > 0) {
			candidateList.resize(candidateQueue.size());
			for (u_int i = candidateList.size() - 1;;) {
				candidateList[i] = candidateQueue.top();
				candidateQueue.pop();

				if (i == 0)
					break;
				--i;
			}

			/*for (u_int i = 0; i < Min<u_int>(candidateList.size(), 10u); ++i) {
				const SimplifyTriangle &t = triangles[candidateList[i].tid];

				SDL_LOG("#" << i << " Min. error: " << fixed << setprecision(10) << t.err[candidateList[i].tvertex] << " (triangle " << candidateList[i].tid << ")");

				const u_int i0 = t.v[candidateList[i].tvertex];
				SimplifyVertex &v0 = vertices[i0];

				const u_int i1 = t.v[(candidateList[i].tvertex + 1) % 3];
				SimplifyVertex &v1 = vertices[i1];

				SDL_LOG("#" << i << " Collapse screen error scale: " << fixed << setprecision(10) <<  CalculateCollapseScreenErrorScale(v0.p, v1.p));
				SDL_LOG("#" << i << " Triangle " << candidateList[i].tid << " border: " <<
						vertices[t.v[candidateList[i].tvertex]].border << " " <<
						vertices[t.v[(candidateList[i].tvertex + 1) % 3]].border);
			}*/

			/*ExtTriangleMeshBuilder meshBuilder;
			for (u_int i = 0; i < candidateList.size(); ++i) {
				const SimplifyTriangle &t = triangles[candidateList[i].tid];

				const u_int index = meshBuilder.vertices.size();
				meshBuilder.AddVertex(vertices[t.v[candidateList[i].tvertex]].p);
				meshBuilder.AddVertex(vertices[t.v[(candidateList[i].tvertex + 1) % 3]].p);
				meshBuilder.AddVertex(vertices[t.v[(candidateList[i].tvertex + 2) % 3]].p);

				meshBuilder.AddTriangle(Triangle(index, index + 1, index +2));
			}
			ExtTriangleMesh *debugMesh = meshBuilder.GetExtTriangleMesh();
			debugMesh->Save("debug-candidates.ply");
			delete debugMesh;*/
		}

		// Clear dirty flag
		for (u_int i = 0; i < triangles.size(); ++i)
			triangles[i].dirty = false;
	}

	// Finally compact mesh before exiting
	void CompactMesh() {
		u_int dst = 0;

		for (u_int i = 0; i < vertices.size(); ++i)
			vertices[i].tcount = 0;

		for (u_int i = 0; i < triangles.size(); ++i) {
			if (!triangles[i].deleted) {
				const SimplifyTriangle &t = triangles[i];
				triangles[dst++] = t;

				vertices[t.v[0]].tcount = 1;
				vertices[t.v[1]].tcount = 1;
				vertices[t.v[2]].tcount = 1;
			}
		}
		triangles.resize(dst);

		dst = 0;
		for (u_int i = 0; i < vertices.size(); ++i) {
			if (vertices[i].tcount) {
				vertices[i].tstart = dst;
				vertices[dst].p = vertices[i].p;

				vertices[dst].norm = vertices[i].norm;
				vertices[dst].uv = vertices[i].uv;
				vertices[dst].col = vertices[i].col;
				vertices[dst].alpha = vertices[i].alpha;

				dst++;
			}
		}

		for (u_int i = 0; i < triangles.size(); ++i) {
			SimplifyTriangle &t = triangles[i];

			t.v[0] = vertices[t.v[0]].tstart;
			t.v[1] = vertices[t.v[1]].tstart;
			t.v[2] = vertices[t.v[2]].tstart;
		}
		vertices.resize(dst);
	}

	// Error between vertex and Quadric
	float VertexError(const SymetricMatrix &q, const float x, const float y, const float z) const {
		return q[0] * x * x + 2.f * q[1] * x * y + 2.f * q[2] * x * z + 2.f * q[3] * x +
				q[4] * y * y + 2.f * q[5] * y * z + 2.f * q[6] * y +
				q[7] * z * z + 2.f * q[8] * z +
				q[9];
	}

	// Error for one edge
	float CalculateCollapseError(const u_int v1Index, const u_int v2Index,
			Point *pResult = nullptr) const {
		const SymetricMatrix q = vertices[v1Index].q + vertices[v2Index].q;

		// Compute interpolated vertex
		const Point &p1 = vertices[v1Index].p;
		const Point &p2 = vertices[v2Index].p;
		const Point p3 = (p1 + p2) / 2;

		// Error can be negative, I add 1 to have screenErrorScale can than
		// work as expected
		const float error1 = VertexError(q, p1.x, p1.y, p1.z) + 1.f;
		const float error2 = VertexError(q, p2.x, p2.y, p2.z) + 1.f;
		const float error3 = VertexError(q, p3.x, p3.y, p3.z) + 1.f;

		float error;
		if (preserveBorder && vertices[v1Index].border) {
			error = error1;
			if (pResult)
				*pResult = p1;
		} else if (preserveBorder && vertices[v2Index].border) {
			error = error2;
			if (pResult)
				*pResult = p2;
		} else {
			error = Min(error1, Min(error2, error3));

			if (pResult) {
				if (error1 == error)
					*pResult = p1;
				if (error2 == error)
					*pResult = p2;
				if (error3 == error)
					*pResult = p3;
			}
		}

		// Adding 1.0 because error have negative values
		return Max(error + 1.f, 0.f);
	}

	float CalculateCollapseScreenErrorScale(const Point &v0, const Point &v1) const {
		if (edgeScreenSize > 0.f) {
			const float notVisibleScale = .5f;

			float v0x, v0y;
			if (!camera->GetSamplePosition(v0, &v0x, &v0y) ||
					!IsValid(v0x) || !IsValid(v0y))
				return notVisibleScale;

			// Normalize
			v0x /= camera->filmWidth;
			v0y /= camera->filmHeight;

			float v1x, v1y;
			if (!camera->GetSamplePosition(v1, &v1x, &v1y) ||
					!IsValid(v1x) || !IsValid(v1y))
				return notVisibleScale;

			// Normalize
			v1x /= camera->filmWidth;
			v1y /= camera->filmHeight;

			const float edge = sqrtf(Sqr(v0x - v1x) + Sqr(v0y - v1y));
			if (edge == 0.f)
				return notVisibleScale;

			return Max(edge / edgeScreenSize, notVisibleScale);
		} else
			return 1.f;
	}

	void UpdateTriangleError(SimplifyTriangle &t) const {
		t.err[0] = CalculateCollapseError(t.v[0], t.v[1]) *
				CalculateCollapseScreenErrorScale(vertices[t.v[0]].p, vertices[t.v[1]].p);

		t.err[1] = CalculateCollapseError(t.v[1], t.v[2]) *
				CalculateCollapseScreenErrorScale(vertices[t.v[1]].p, vertices[t.v[2]].p);

		t.err[2] = CalculateCollapseError(t.v[2], t.v[0]) *
				CalculateCollapseScreenErrorScale(vertices[t.v[2]].p, vertices[t.v[0]].p);
	}
};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

SimplifyShape::SimplifyShape(const Camera *camera, ExtTriangleMesh *srcMesh,
		const float target, const float edgeScreenSize, const bool preserveBorder) {
	SDL_LOG("Simplify shape " << srcMesh->GetName() << " with target " << target);

	if ((edgeScreenSize > 0.f) && !camera)
		throw runtime_error("The scene camera must be defined in order to enable simplify edgescreensize option");

	const float startTime = WallClockTime();

	const u_int targetCount = Max(1u, Floor2UInt(srcMesh->GetTotalTriangleCount() * target));

	/*srcMesh->Save("debug-start.ply");
	ExtTriangleMesh *debugMeshStart = ScreenProjection(*camera, *srcMesh);
	debugMeshStart->Save("debug-start-proj.ply");
	delete debugMeshStart;*/

	Simplify simplify(*srcMesh);
	simplify.Decimate(targetCount, camera, edgeScreenSize, preserveBorder);
	mesh = simplify.GetExtMesh();

	/*srcMesh->Save("debug-end.ply");
	ExtTriangleMesh *debugMeshEnd = ScreenProjection(*camera, *mesh);
	debugMeshEnd->Save("debug-end-proj.ply");
	delete debugMeshEnd;*/

	SDL_LOG("Subdivided shape from " << srcMesh->GetTotalTriangleCount() << " to " << mesh->GetTotalTriangleCount() << " faces");

	// For some debugging
	//mesh->Save("debug.ply");

	const float endTime = WallClockTime();
	SDL_LOG("Simplify time: " << (boost::format("%.3f") % (endTime - startTime)) << "secs");
}

SimplifyShape::~SimplifyShape() {
	if (!refined)
		delete mesh;
}

ExtTriangleMesh *SimplifyShape::RefineImpl(const Scene *scene) {
	return mesh;
}


#include <safety_module/SafetyModule.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <ros/ros.h>

SafetyModule::SafetyModule(double robotRadius, double minimumDistanceToRobot, double timeOffset,
                           double maximumDeceleration)
    : _robotRadius(robotRadius), _minimumDistanceToRobot(minimumDistanceToRobot),
      _timeOffset(timeOffset), _maxDeceleration(maximumDeceleration)
{
}

bool SafetyModule::motionIsSafe(const Eigen::Vector2d& currentPosition,
                                const Eigen::Quaterniond& currentOrientation, const double transVel,
                                const double rotVel,
                                const std::vector<Eigen::Vector2d>& pointsLaser)
{

  if (transVel < 0) {
    ROS_WARN_STREAM_NAMED("safety_module",
                          "TransVel is negative: " << transVel << ". This is currently unsafe, but not prevented!");
    return true;
  }

  std::ostringstream os;
  _distSensorToEdge = _robotRadius - abs(currentPosition.norm());
  double yaw        = currentOrientation.matrix().eulerAngles(0, 1, 2)[2];
  Eigen::Vector2d currentHeadingVector(cos(yaw), sin(yaw));
  Eigen::Vector2d positionRobotStart, positionRobotEnd, positionCenter;
  positionRobotStart = currentPosition;

  // Compute stopping distance of the robot
  const double distStop = computeStoppingDistance(transVel);
  bool collision        = false;
  bool objectIsClose    = false;

  double minLaserDistSquared = std::numeric_limits<double>::max();
  double distSquared;
  for (const Eigen::Vector2d& laserMeas : pointsLaser) {
    objectIsClose       = objectTooClose(laserMeas, positionRobotStart, distSquared);
    minLaserDistSquared = distSquared < minLaserDistSquared ? distSquared : minLaserDistSquared;
    if (objectIsClose)
      _motionIsSafe = false;
  }

  if (!_motionIsSafe)
    os << "Object too close, dist to center: " << sqrt(minLaserDistSquared) << ". ";

  // Compute center of rotation for current motion
  if (_motionIsSafe) {
    ROS_DEBUG_STREAM("distStop: " << distStop << ", transVel: " << transVel
                                  << ", rotVel: " << rotVel);
    if (abs(rotVel) <= _rotVelThreshold) {
      for (const Eigen::Vector2d& laserMeas : pointsLaser) {
        if (transVel > 0) {
          collision =
              (std::abs(laserMeas(1)) < _robotRadius + _minimumLateralDistance) &&
              (laserMeas(0) < distStop + _distSensorToEdge + _minimumLongitudinalDistance) &&
              (laserMeas(0) > positionRobotStart(1));
          if (collision)
            ROS_DEBUG_STREAM_NAMED(
                "safety_module",
                "Anticipated collision (straight forward driving). Stopping distance is "
                    << distStop);
        } else if (transVel < 0) {
          collision =
              (std::abs(laserMeas(1)) < _robotRadius + _minimumLateralDistance) &&
              (laserMeas(0) < positionRobotStart(1)) &&
              (laserMeas(0) > -(distStop + _distSensorToEdge + _minimumLongitudinalDistance));
          if (collision)
            ROS_DEBUG_STREAM_NAMED(
                "safety_module",
                "Anticipated collision (straight backward driving). Stopping distance is "
                    << distStop);
        }

        if (collision)
          _motionIsSafe = false;
      }
    } else {
      const double radiusRobotTrajectory = computeTrajectoryRadius(transVel, rotVel);

      if (radiusRobotTrajectory > _robotRadius) {
        positionCenter = computeRotationCenterOfTrajectory(
            radiusRobotTrajectory, transVel, rotVel, positionRobotStart, currentHeadingVector);
        positionRobotEnd =
            computeStoppingPoint(transVel, rotVel, positionRobotStart, positionCenter);

        // generate hyperplanes that form the limits of the critical collision region
        auto hyperCenterStart =
            Eigen::Hyperplane<double, 2>::Through(positionCenter, positionRobotStart);
        auto hyperCenterEnd =
            Eigen::Hyperplane<double, 2>::Through(positionCenter, positionRobotEnd);

        // find sign of the distance (critical region on which side of the halfspace) with respect
        // to the halfspaces
        int signForCollisionCenterStart = getSignOfDistance(hyperCenterStart, positionRobotEnd);
        int signForCollisionCenterEnd   = getSignOfDistance(hyperCenterEnd, positionRobotStart);

        for (const Eigen::Vector2d& laserMeas : pointsLaser) {
          double distCenterSquared = (laserMeas - positionCenter).squaredNorm();
          collision =
              (distCenterSquared <=
               (radiusRobotTrajectory + _robotRadius + _minimumLateralDistance) *
                   (radiusRobotTrajectory + _robotRadius +
                    _minimumLateralDistance)) &&  // within sector of circle
              (distCenterSquared >=
               (radiusRobotTrajectory - _robotRadius - _minimumLateralDistance) *
                   (radiusRobotTrajectory - _robotRadius - _minimumLateralDistance)) &&
              (signForCollisionCenterStart ==
               getSignOfDistance(hyperCenterStart, laserMeas)) &&  // within halfspace limits
              (signForCollisionCenterEnd == getSignOfDistance(hyperCenterEnd, laserMeas));
          if (collision) {
            _motionIsSafe = false;
            break;
          }
        }
      }
    }

    if (!_motionIsSafe)
      os << "Collision with command tv: " << transVel << ", rv: " << rotVel
         << ", brake dist: " << distStop << ". ";
  }

  if (_motionIsSafe)
    os << "Motion is safe. ";

  _statusMsg += os.str();
  ROS_DEBUG_STREAM_COND(!_motionIsSafe, _statusMsg);

  return _motionIsSafe;
}

double SafetyModule::computeStoppingDistance(const double transVel)
{
  double absVel = std::abs(transVel);
  return absVel * _timeOffset + absVel * absVel / (2 * _maxDeceleration) + _robotRadius;
}

Eigen::Vector2d SafetyModule::transformPointPolar2KarthesianCoordinates(const double range,
                                                                        const double alpha)
{
  Eigen::Vector2d laserMeas(cos(alpha) * range, sin(alpha) * range);
  return laserMeas;
}

int SafetyModule::getSignOfDistance(const Eigen::Hyperplane<double, 2>& hyperplane,
                                    const Eigen::Vector2d& point)
{
  return sign(hyperplane.signedDistance(point));
}

Eigen::Vector2d SafetyModule::computeStoppingPoint(const double transVel, const double rotVel,
                                                   const Eigen::Vector2d& positionStart,
                                                   const Eigen::Vector2d& positionCenter)
{
  double dTheta = (computeStoppingDistance(transVel) + _minimumLongitudinalDistance) /
                  abs(transVel / rotVel) * sign(rotVel);
  Eigen::Rotation2D<double> rotationMatrix(dTheta);
  return positionCenter + rotationMatrix * (positionStart - positionCenter);
}

Eigen::Vector2d SafetyModule::computeRotationCenterOfTrajectory(
    const double radius, const double transVel, const double rotVel,
    const Eigen::Vector2d& positionStart, const Eigen::Vector2d& currentHeading)
{
  Eigen::Vector2d rotatedHeading90Deg(-currentHeading(1), currentHeading(0));
  return positionStart +
         sign(rotVel) * sign(transVel) * radius *
             rotatedHeading90Deg;  // center on the left if positive rotation rate
}

double SafetyModule::computeTrajectoryRadius(const double transVel, const double rotVel)
{
  double radius;
  if (std::abs(rotVel) < _rotVelThreshold) {
    radius = 1e4;
  } else {
    radius = std::abs(transVel / rotVel);
  }
  return std::max(0.001, radius);
}

bool SafetyModule::objectTooClose(const Eigen::Vector2d& laserMeas,
                                  const Eigen::Vector2d& positionRobot,
                                  double& laserPointDistToCenterSquared)
{
  laserPointDistToCenterSquared = (laserMeas - positionRobot).squaredNorm();
  bool tooCloseToRobotCenter =
      (laserPointDistToCenterSquared <=
       (_robotRadius + _minimumDistanceToRobot) * (_robotRadius + _minimumDistanceToRobot));
  bool tooCloseToSensor(laserMeas.norm() <= _minimumDistanceToSensor);
  ROS_DEBUG_STREAM_COND_NAMED(tooCloseToSensor || tooCloseToRobotCenter, "safety_module",
                              "Distance to robot center is "
                                  << sqrt(laserPointDistToCenterSquared));
  ROS_DEBUG_STREAM_COND_NAMED(tooCloseToSensor || tooCloseToRobotCenter, "safety_module",
                              "Distance to sensor is " << laserMeas.norm());
  ROS_DEBUG_STREAM_COND_NAMED(tooCloseToSensor || tooCloseToRobotCenter, "safety_module",
                              "Object is too close to sensor: " << tooCloseToSensor
                                                                << ", too close to robot center: "
                                                                << tooCloseToRobotCenter);
  return tooCloseToRobotCenter || tooCloseToSensor;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  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_COMMON_FUNCTION_GENERIC_COSH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_GENERIC_COSH_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/average.hpp>
#include <boost/simd/function/simd/exp.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_greater.hpp>
#include <boost/simd/function/simd/rec.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( cosh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) according x < Threshold e =  exp(x) or exp(x/2) is
      // respectively computed
      // *  in the first case cosh (e+rec(e))/2
      // *  in the second     cosh is (e/2)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      auto test1 = (x > Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = Half<A0>()*tmp;
      return if_else(test1, tmp1*tmp, bs::average(tmp, rec(tmp)));
    }
  };
} } }

#endif


#include <iostream>

#include <boost/filesystem.hpp>

#include "pagmo/problems/cec2013.hpp"
#include "pagmo/island.hpp"
#include "pagmo/problem.hpp"

#include "Problems/himmelblau.h"
#include "Problems/getAlgorithm.h"
#include "Problems/saveOptimizationResults.h"
#include "Problems/applicationOutput.h"

int main( )
{
    // Set random seed
    pagmo::random_device::set_seed( 12345 );

    // Define run settings
    unsigned int numberOfDimensionCases = 3;
    unsigned int numberOfOptimizers = 11;
    unsigned int numberOfProblems = 28;
    unsigned int numberOfPopulationCases = 3;

    // Create double vector of Matrices that contain optimal solutions
    std::vector< std::vector< Eigen::MatrixXd > > optima;
    optima.resize( numberOfDimensionCases );
    for( unsigned int i = 0; i < numberOfDimensionCases; i++ )
    {
        optima[ i ].resize( numberOfPopulationCases );
        for( unsigned int j = 0; j < numberOfPopulationCases; j++ )
        {
            optima[ i ][ j ] = Eigen::MatrixXd( numberOfProblems, numberOfOptimizers );
        }
    }

    // Perform optimization for each optimizer
    for( unsigned int i = 0; i < numberOfOptimizers; i++ )
    {
        // Perform optimization for each test problem
        for( unsigned int j = 0; j < numberOfProblems; j++ )
        {
            // Perform optimization for each dimensionality
            for( unsigned int k = 0; k < numberOfDimensionCases; k++ )
            {
                int numberOfDimensions = 0;
                if( k == 0 )
                {
                    numberOfDimensions = 2;
                }
                else if( k == 1 )
                {
                    numberOfDimensions = 5;
                }
                else if( k == 2 )
                {
                    numberOfDimensions = 10;
                }

                // Perform optimization for each population size/number of generations
                for( unsigned int l = 0; l < numberOfPopulationCases; l++ )
                {
                    pagmo::population::size_type populationSize = 0;
                    int numberOfGenerations = 0;
                    if( l == 0 )
                    {
                        populationSize = 16;
                        numberOfGenerations = 64;
                    }
                    else if( l == 1 )
                    {
                        populationSize = 32;
                        numberOfGenerations = 32;
                    }
                    else if( l == 2 )
                    {
                        populationSize = 64;
                        numberOfGenerations = 16;
                    }

                    // Create test problem
                    pagmo::problem prob{ pagmo::cec2013( j + 1, numberOfDimensions ) };

                    // Create optimization algorithm
                    pagmo::algorithm algo = getAlgorithm( i );

                    // Create island with given population size
                    pagmo::island isl = pagmo::island{ algo, prob, populationSize };

                    // Evolve for required number of generations
                    for( int j = 1; j <= numberOfGenerations; j++ )
                    {
                        isl.evolve( );
                        while( isl.status( ) != pagmo::evolve_status::idle &&
                               isl.status( ) != pagmo::evolve_status::idle_error )
                        {
                            isl.wait( );
                        }
                        isl.wait_check( ); // Raises errors
                    }

                    // Save optimal results for current optimization.
                    optima[ k ][ l ]( j, i ) = isl.get_population().champion_f()[0];
                    std::cout << "Minimum: " << i<<" "<<" "<<j<<" "<<isl.get_population().champion_f()[0] << std::endl;
                }
            }
            std::cout<<std::endl;
        }
    }

    // Write all results to files.
    for( unsigned int i = 0; i < numberOfDimensionCases; i++ )
    {
        for( unsigned int j = 0; j < numberOfPopulationCases; j++ )
        {
            tudat::input_output::writeMatrixToFile( optima[ i ][ j ], "cec2013Optima_" + std::to_string( i ) + "_" +
                                                    std::to_string( j ) + ".dat", 16,
                                                    tudat_pagmo_applications::getOutputPath( ) );
        }
    }

    return 0;
}


#include <DES.h>
#include <NTL/ZZ.h>
#include <X917.h>
#include <bitset>
#include <ctime>
#include <random>
using std::bitset;
using namespace NTL;
using namespace DES;
ZZ X917rand(bitset<64> const &seed, int m) {
	Bit64 k1(rand()), k2(rand());
	Bit64 s(seed);
	Bit64 D(bitset<64>(time(NULL)));
	Bit64 l = encipher(decipher(encipher(D, k1), k2), k1);
	Bit64 x;
	ZZ ret(0);
	for (int i = 0; i < m; i++) {
		x = encipher(decipher(encipher(Bit64(l ^ s), k1), k2), k1);
		s = encipher(decipher(encipher(Bit64(l ^ x), k1), k2), k1);
		ret <<= 32;
		ret |= x.content.to_ullong()>>32;
		ret <<= 32;
		ret |= x.content.to_ullong()&(0xffffffff);
	}
	return ret;
}


#include <smtbx/refinement/constraints/u_eq_dependent_u_iso.h>

#include <boost/lambda/lambda.hpp>
#include <scitbx/array_family/tiny_algebra.h>

namespace smtbx { namespace refinement { namespace constraints {

  void
  u_iso_proportional_to_pivot_u_eq
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    af::double6 f = unit_cell.u_star_to_u_iso_linear_form();
    f *= multiplier;
    u_star_parameter *u = pivot_u();
    value = f * u->value;

    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    for (int j=0; j<6; ++j) {
      jt.col(index()) = jt.col(index()) + f[j]*jt.col(u->index() + j);
    }
  }

}}}


/**
 * This is a simple unoptimized code to evaluate some low-order dual diagrams in FK model
 * input :
 *  --order : order of the diagram
 *  --beta : order of the diagram
 *  --kpts : order of the diagram
 *  --U : order of the diagram
 * output:
 *  - dual self-energy "sigma_wk.dat"
 *  - cut of dual self-energy at first Matsubara freq
 *  - k-dependence of dual bubbles (summer over Matsubara freqs).
 */

#include <boost/program_options.hpp>
#include <gftools.hpp>
#include <Eigen/Core>

namespace po = boost::program_options;
using namespace gftools;

int main(int argc, char *argv[])
{
    // parse command line options - define beta, kpts and U
    po::options_description desc("FK DF 2d");
    desc.add_options()
        ("order,n", po::value<int>()->default_value(2), "order of diagrams")
        ("beta", po::value<double>()->default_value(1), "inverse temperature")
        ("kpts", po::value<int>()->default_value(16), "number of points in one dimension of k-space")
        ("U", po::value<double>()->default_value(8), "interaction strength")
        ("help", "produce help message");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    //po::notify(vm);
    if (vm.count("help")) { std::cout << desc << std::endl; exit(0); }
    int diagram_order = vm["order"].as<int>();
    int kpts = vm["kpts"].as<int>();
    // we will use diagram_order as amount of iterations in vertices. 1 iteration = 2nd order, etc.
    diagram_order-=(diagram_order>0);
    double beta = vm["beta"].as<double>();
    double U = vm["U"].as<double>();
    static constexpr int NDim = 2; // work in 2 dimensions

    // Calc starts here
    std::cout << "Atomic limit of FK model in 2 dimensions using DF approximation" << std::endl;
    double T = 1.0/beta;
    std::cout << "beta = " << beta << std::endl;
    std::cout << "T = " << T << std::endl;
    int totalkpts = std::pow(kpts, NDim);
    std::cout << "kmesh : " << kpts << " points " << std::endl;
    std::cout << "total pts in BZ = " << totalkpts << std::endl;

    // typedef for F(w, w') type objects
    typedef grid_object<std::complex<double>, fmatsubara_grid, fmatsubara_grid> vertex_type;
    // G(w)
    typedef grid_object<std::complex<double>, fmatsubara_grid> gw_type;
    // G(w, kx, ky)
    typedef grid_object<std::complex<double>, fmatsubara_grid, kmesh, kmesh> gk_type;
    // e(kx, ky)
    typedef grid_object<std::complex<double>, kmesh, kmesh> disp_type;
    // typedef for matrices
    typedef Eigen::MatrixXcd matrix_type;

    // Prepare evaluation grids
    fmatsubara_grid fgrid(-20,20,beta);
    // Hybridization function (atomic limit)
    gw_type delta(fgrid);
    delta.fill([&](std::complex<double> w){return double(2*NDim) / w; });
    // Local gf (atomic limit)
    gw_type gw(fgrid);
    gw.fill([&](std::complex<double> w){return 0.5/(w - U/2.0) + 0.5/(w + U/2.0);});
    // Local vertex (atomic limit)
    vertex_type gamma4(std::make_tuple(fgrid, fgrid));
    typename gw_type::function_type Lambda = [U](std::complex<double> w){return 1. - U*U/4./w/w;};
    gamma4.fill([&](std::complex<double> w1, std::complex<double> w2){
            return beta * U * U / 4.0 * double(1 - tools::is_float_equal(w1, w2) ) * (1. - U*U/4./w1/w1) * (1. - U*U/4./w2/w2);
        });

    // k-dependent input
    // define a mesh in Brilloin zone (BZ)
    kmesh kgrid(kpts);
    // lattice dispersion
    disp_type eps(std::forward_as_tuple(kgrid, kgrid));
    eps.fill([&](double kx, double ky){return -2*(cos(kx) + cos(ky));});
    // lattice gf in dmft
    gk_type glat_dmft(std::forward_as_tuple(fgrid, kgrid, kgrid));
    glat_dmft.fill([&](fmatsubara_grid::point w, kmesh::point kx, kmesh::point ky){return 1.0 / (1.0 / gw(w) + delta(w) - eps(kx, ky));});
    // bare dual df
    gk_type gd0(glat_dmft.grids());
    gd0.fill([&](fmatsubara_grid::point w, kmesh::point kx, kmesh::point ky){return glat_dmft(w, kx, ky) - gw(w);});

    double prec = 1e-5;
    // check - local part of gd0 should be zero (check is approximate, as atomic limit is used)
    for (auto w : fgrid.points()) {
        double diff0 = std::abs(gd0[w].sum()) / double(totalkpts);
        if (diff0 > prec) std::cerr << "Problem encountered - w = " << w << ": dual gd0 has a non-zero local part : " <<  diff0 << " > " << prec << std::endl;
        }

    // define full vertex (will be evaluated below)
    vertex_type full_vertex(gamma4);
    // make a matrix from the vertex
    matrix_type gamma4_matrix = gamma4.data().as_matrix();
    matrix_type full_vertex_matrix(gamma4_matrix);

    // check that we are consistent - 2 frequency grids are the same
    if (gd0.template grid<0>() != fgrid) throw std::logic_error("matsubara grid mismatch");

    std::cout << "kmesh : " << kgrid << std::endl;
    // Initalize dual self-energy on the same grids as gd0
    gk_type sigma_dual(gd0.grids());
    sigma_dual = 0.0;
    disp_type bare_bubbles(kgrid,kgrid);

    // now loop through the BZ (no irreducible part optimization)
    for (kmesh::point q1 : kgrid.points()) {
        for (kmesh::point q2 : kgrid.points()) {
            std::cout << "[" << q1.index()*kpts + q2.index()+1 << "/" << totalkpts <<  "]; q = {" <<  q1.value() << " " << q2.value() << "}" << std::endl;
            // Evaluate -T \sum_k G_{w,k} G_{w,k+q}
            // Shift dual g in k-space and make no frequency shift.
            // note : this operation is typically optimized via an fft.
            gk_type gd0_shift = gd0.shift(std::make_tuple(0.0,q1,q2));
            // obtain a bubble
            gk_type bubble_wk = -T * gd0 * gd0_shift;
            // perform sum over k
            gw_type dual_bubble(fgrid);
            for (auto w : fgrid.points()) { dual_bubble[w] = bubble_wk[w].sum() / double(totalkpts); }
            // save the bubble for output
            bare_bubbles(q1, q2) = dual_bubble.sum();
            // construct a diagonal matrix (in frequency space from the bubble)
            matrix_type dual_bubble_matrix = dual_bubble.data().as_diagonal_matrix();
            // refresh vertex
            full_vertex_matrix = gamma4_matrix;
            // IMPORTANT : Evaluate dual diagram
            for (int n=0; n<diagram_order; n++)
                full_vertex_matrix= gamma4_matrix * dual_bubble_matrix * full_vertex_matrix;
            // update self-energy
            for (auto w : fgrid.points())
                sigma_dual[w.index()] += T* full_vertex_matrix(w.index(), w.index()) * gd0_shift[w.index()] / double(totalkpts);
        }
    }
    // output
    // save bare bubbles
    bare_bubbles.savetxt("db0.dat");
    // save sigma
    sigma_dual.savetxt("sigma_wk.dat");
    // save sigma at first matsubara
    auto w0 = fgrid.find_nearest(I*PI/beta);
    disp_type sigma_w0(std::forward_as_tuple(kgrid,kgrid),sigma_dual[w0]);
    sigma_w0.savetxt("sigma_w0.dat");

    // save lattice self-energy
    gk_type sigma_lat(sigma_dual.grids());
    for (fmatsubara_grid::point w : fgrid.points()) {
        sigma_lat[w] = sigma_dual[w] / (sigma_dual[w] * gw[w] + 1.0);
        }
    sigma_lat.savetxt("sigma_lattice.dat");
    sigma_w0.data() = sigma_lat[w0];
    sigma_w0.savetxt("sigma_lattice_w0.dat");
}