#include <navigation_features/nav_features.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Core>
#include <ros/console.h>
#include <sys/time.h>

using namespace std;

features::NavFeatures::NavFeatures() {}

features::NavFeatures::NavFeatures(tf::TransformListener* tf, float size_x, float size_y, float res)
{
	tf_listener_ = tf;
	size_x_ = size_x*2.0;  	//m
	size_y_ = size_y*2.0;  	//m
	resolution_ = res;		//m/cell
	max_planning_dist_ = sqrt((size_x_*size_x_)+(size_y_*size_y_));
	insc_radius_robot_ = 0.40;

	use_loss_func_ = false;
	demo_path_.clear();

	setParams();

}

features::NavFeatures::NavFeatures(tf::TransformListener* tf, vector<geometry_msgs::Point>* footprint, float insc_radius, float size_x, float size_y, float res)
{

	tf_listener_ = tf;
	insc_radius_robot_ = insc_radius;
	size_x_ = size_x*2.0;  	//m
	size_y_ = size_y*2.0;  	//m
	resolution_ = res;		//m/cell
	max_planning_dist_ = sqrt((size_x_*size_x_)+(size_y_*size_y_));

	myfootprint_ = footprint;

	use_loss_func_ = false;
	demo_path_.clear();

	setParams();

}

void features::NavFeatures::setParams()
{
	//Read the ROS params from the server
	ros::NodeHandle n("~/Navigation_features");

	//Dynamic reconfigure
	//dsrv_ = new dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>(n); //ros::NodeHandle("~")
    //dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>::CallbackType cb = boost::bind(&NavFeatures::reconfigureCB, this, _1, _2);
    //dsrv_->setCallback(cb);

	//Load weights "w1, w2, ..."
	w_.clear();
	bool ok = true;
	unsigned int i = 1;
	while(ok)
	{
		char buf[10];
		sprintf(buf, "w%u", i);
		string st = string(buf);

		if(n.hasParam(st.c_str())){
			double wg = 0.0;
			n.getParam(st.c_str(), wg);
			w_.push_back((float)wg);
			printf("NavFeatures. weight %u= %.3f loaded\n", i, wg);
		} else {
			//printf("param '%s' not found\n", st.c_str());
			ok = false;
		}
		i++;
	}

	n.param<int>("upo_featureset", upo_featureset_, 0);

	n.param<bool>("use_global_map", use_global_map_, false);

	string pc_topic;
	n.param<string>("pc_topic", pc_topic, string("/scan360/point_cloud"));
	int pc_type;
	n.param<int>("pc_type", pc_type, 2); //1->PointCloud, 2->PointCloud2

	max_cost_obs_ = distance_functions(0.0, INVERSE_DEC); //EXP_DEC //INVERSE_DEC
	if(use_global_map_)
		setupMapProjection();
	else
		setupNoMapProjection();

	double front;
	n.param<double>("stddev_person_front", front, 1.2);
	double aside;
	n.param<double>("stddev_person_aside", aside, (1.2/1.5)); //=0.8

	double right;
	n.param<double>("stddev_person_right", right, 0.8);

	n.param<bool>("enable_grouping", grouping_, true);
	double aux;
	n.param<double>("stddev_group", aux, 0.8);
	stddev_group_ = (float)aux;
	double aux2;
	n.param<double>("grouping_distance", aux2, 1.5);
	grouping_distance_ = (float)aux2;

	amp_ = 1.80;			//Amplitude, max value of the gaussian
	sigmas_.push_back((float)front);  //front of person. front
	sigmas_.push_back((float)aside); //aside of person. front
	sigmas_.push_back(sigmas_.at(1)); //front of person. back
	sigmas_.push_back(sigmas_.at(1)); //aside of person. back
	sigmas_.push_back(right); //right of person. front
	sigmas_.push_back(right/2.5); //right of person. aside

	n.param<double>("approaching_angle", aux, 0.40);
	approaching_angle_ = (float)aux;

	it_remove_gauss_ = false;

	//Goal subscription
	//n.param<int>("goal_type", goal_type_, 1); //1->RRT, 2->A*
	//if(goal_type_ == 1)
		goal_sub_ = nh_.subscribe("/rrt_goal", 1, &NavFeatures::goalCallback, this);
	//else
		//goal_sub_ = nh_.subscribe("/move_base/current_goal", 1, &NavFeatures::goalAstarCallback, this);

	//People subscription
	sub_people_ = nh_.subscribe("/people/navigation", 1, &NavFeatures::peopleCallback, this);

	//Obstacles subscription
	if(pc_type == 1) 	//pointCloud
		sub_pc_ = nh_.subscribe(pc_topic, 1, &NavFeatures::pcCallback, this);
	else   				//pointCloud2
		sub_pc_ = nh_.subscribe(pc_topic, 1, &NavFeatures::pc2Callback, this);

	pub_gaussian_markers_ = n.advertise<visualization_msgs::MarkerArray>("gaussian_markers", 5);

	ros::NodeHandle nd("navigation_features");
	//ros::ServiceServer service = nd.advertiseService("setApproachingIT", setApproachingIT);
	loss_srv_ = nd.advertiseService("set_use_loss_func", &features::NavFeatures::setLossService, this);
	valid_srv_ = nd.advertiseService("is_pose_valid", &features::NavFeatures::isPoseValidService, this);
	weights_srv_ = nd.advertiseService("setWeights", &features::NavFeatures::setWeightsService, this);
	init_weights_srv_ = nd.advertiseService("initWeights", &features::NavFeatures::initializeWeightsService, this);
	scenario_srv_ = nd.advertiseService("setScenario", &features::NavFeatures::setScenarioService, this);
	features_srv_ = nd.advertiseService("getPathFeatureCount", &features::NavFeatures::getFeatureCountService, this);

	//Dynamic reconfigure
	dsrv_ = new dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>(n); //ros::NodeHandle("~")
    dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>::CallbackType cb = boost::bind(&NavFeatures::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);

}

features::NavFeatures::~NavFeatures() {

}

 void features::NavFeatures::reconfigureCB(navigation_features::nav_featuresConfig &config, uint32_t level){

    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
	//configuration_mutex_.lock();

    upo_featureset_ = config.upo_featureset;
	//use_global_map_ = config.use_global_map;
	sigmas_[0] = config.stddev_person_front;
	sigmas_[1] = config.stddev_person_aside;
	sigmas_[2] = sigmas_[1];
	sigmas_[3] = sigmas_[1];
	sigmas_[4] = config.stddev_person_right;
	sigmas_[5] = sigmas_[4]/2.5;
	grouping_ = config.enable_grouping;
	stddev_group_ = config.stddev_group;
	grouping_distance_ = config.grouping_distance;
	it_id_ = config.interaction_target_id;
	it_remove_gauss_ = config.it_remove_gaussian;
	//printf("upo_featureset. it_id:%i\n", it_id_);

	//configuration_mutex_.unlock();
}

//Service
bool features::NavFeatures::setWeightsService(navigation_features::SetWeights::Request  &req, navigation_features::SetWeights::Response &res)
{
	setWeights(req.weights);
	return true;
}

//Service
bool features::NavFeatures::setLossService(navigation_features::SetLossCost::Request &req, navigation_features::SetLossCost::Response &res)
{
	printf("NavFeatures. Enabling loss function: %i\n", (int)req.use_loss_func);
	set_use_loss_func(req.use_loss_func);
	set_demo_path(req.demo_path);

	return true;
}

//Service
bool features::NavFeatures::isPoseValidService(navigation_features::PoseValid::Request &req, navigation_features::PoseValid::Response &res)
{
	geometry_msgs::PoseStamped p = req.pose;
	res.ok = poseValid(&p);
}

//Service
bool features::NavFeatures::setScenarioService(navigation_features::SetScenario::Request &req, navigation_features::SetScenario::Response &res)
{
	setScenario(req.obstacles, req.people, req.goal);

	return true;
}

//Service
bool features::NavFeatures::getFeatureCountService(navigation_features::GetFeatureCount::Request &req, navigation_features::GetFeatureCount::Response &res)
{
	res.fc = getPathFeatureCount(&req.path);
	return true;
}

//Service
bool features::NavFeatures::initializeWeightsService(navigation_features::InitWeights::Request &req, navigation_features::InitWeights::Response &res)
{
	vector<float> w;
	if(req.random) {
		srand(time(NULL));
		for(int i=0; i<w_.size(); i++){
			float v = rand() % 10 + 2;
			w.push_back(v);
		}
	} else {
		w.assign((int)w_.size(), 0.0);
	}

	//Normalize weights
	if(req.normalize && req.random) {
		float total = accumulate(w.begin(), w.end(), 0);
		for(unsigned int i=0; i<w.size(); i++)
			w[i] = w[i]/total;
	}
	w_ = w;
	res.weights = w;
	return true;
}

void features::NavFeatures::setScenario(sensor_msgs::PointCloud2 obs, upo_msgs::PersonPoseArrayUPO people, geometry_msgs::PoseStamped goal)
{
	setGoal(goal);
	setPeople(people);
	setObstacles(obs);
}

void features::NavFeatures::setObstacles(sensor_msgs::PointCloud2 obs)
{
	//printf("NavFeatures. Setting obstacles!\n");
	sensor_msgs::PointCloud2 lcloud;
	obs.header.stamp = ros::Time();
	try{
		if(use_global_map_) {
			if(!pcl_ros::transformPointCloud("/map", obs, lcloud, *tf_listener_))
				ROS_WARN("TransformPointCloud failed!!!!!");
		} else {
			if(!pcl_ros::transformPointCloud("/base_link", obs, lcloud, *tf_listener_))
				ROS_WARN("TransformPointCloud failed!!!!!");
		}

	} catch (tf::TransformException ex){
		ROS_WARN("NAV FEATURES. pcCallback. TransformException: %s", ex.what());
	}

	laserMutex_.lock();
	laser_cloud_ = lcloud;
	laserMutex_.unlock();
}

/**
 * Calculate the loss function
 * Input:
 	x, point coordinate
	y, point coordinate
	example_path, demonstration path
 * Output:
	loss_value in the range [0, 0.5]
 */
float features::NavFeatures::calculate_loss_function(geometry_msgs::PoseStamped* p)
{
	if(demo_path_.empty())
	{
		printf("\n	NavFeatures: ERRORRRRRRR! DEMONSTRATION PATH IS EMPTY!!!!!\n");
		return 0.0;
	}
	if(!p) {
		printf("\n	NavFeatures: ERRORRRRRRR! point is null!!!!!\n");
		return 0.0;
	}

	geometry_msgs::PoseStamped point = *p;
	//printf("calculate loss function. header: %s\n", point.header.frame_id.c_str());

	if(point.header.frame_id.empty() || point.header.frame_id.length() < 3)
		printf("\n¡¡¡¡¡¡¡¡¡¡¡¡¡¡¡calculate_loss_function. global_frame invalid!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n");

	//Just in case
	if(point.header.frame_id.compare("/odom") != 0 && point.header.frame_id.compare("odom") != 0)
	{
		//printf("calculate loss function. header: %s x:%.2f y:%.2f, th:%2f\n", point.header.frame_id.c_str(), point.pose.position.x, point.pose.position.y, tf::getYaw(point.pose.orientation));
		point = transformPoseTo(point, "odom", false); //true
	}
	float x = point.pose.position.x;
	float y = point.pose.position.y;

	float min_dist = 9999.0;
	for(unsigned int i=0; i<demo_path_.size(); i++)
	{
		geometry_msgs::PoseStamped ps = demo_path_[i];
		//Just in case
		if(ps.header.frame_id.compare("/odom") != 0 && ps.header.frame_id.compare("odom") != 0)
		{
			printf("calculate loss function. Demo path!! header: %s x:%.2f y:%.2f, th:%2f\n", ps.header.frame_id.c_str(), ps.pose.position.x, ps.pose.position.y, tf::getYaw(ps.pose.orientation));
			ps = transformPoseTo(ps, "odom", false); //true
		}

		float xe = ps.pose.position.x;
		float ye = ps.pose.position.y;
		float d = sqrt((x-xe)*(x-xe) + (y-ye)*(y-ye));
		if(d < min_dist)
			min_dist = d;
	}

	if(min_dist > 1.0)
		min_dist = 1.0;

	return min_dist;

	//The cost decreases linearly from 1 to 0 in a distance of 1 meter
	//return (float)(1-min_dist)/2.0;

}

void features::NavFeatures::setupMapProjection()
{

	people_paint_area_ = 25;

	ros::ServiceClient map_client = nh_.serviceClient<nav_msgs::GetMap> ("/static_map");
	while (! ros::service::waitForService("/static_map",1)){
		ROS_INFO("Waiting for map service");
	}

	nav_msgs::GetMap srv;
	map_client.call(srv);
	ROS_INFO_STREAM(srv.response.map.info);
	map_image_ = cv::Mat(srv.response.map.info.height, srv.response.map.info.width,CV_8UC1, cv::Scalar(0));
	map_metadata_ =srv.response.map.info;
	resolution_ = (double) map_metadata_.resolution;
	origin_.push_back(map_metadata_.origin.position.x); //m
	origin_.push_back(map_metadata_.origin.position.y); //m
	origin_.push_back(tf::getYaw(map_metadata_.origin.orientation));
	uint8_t *myData = map_image_.data;
	for (int i=0;i<srv.response.map.data.size();i++){
		if (srv.response.map.data.at(i)==100  || srv.response.map.data.at(i)==-1 ){
		}
		else {
			map_image_.data[i] = 255;
		}
	}

	dtMutex_.lock();
	distance_transform_ = cv::Mat(map_image_.rows,map_image_.cols,CV_32FC1);
	cv::distanceTransform(map_image_,distance_transform_,CV_DIST_L1,3);
	dtMutex_.unlock();

}

void features::NavFeatures::setupNoMapProjection()
{
	people_paint_area_ = 25;

	map_image_ = cv::Mat((int)size_x_/resolution_, (int)size_y_/resolution_, CV_8UC1, cv::Scalar(255));
	origin_.push_back(size_x_/2.0);
	origin_.push_back(size_y_/2.0);
	origin_.push_back(0.0);

	map_metadata_.resolution = resolution_;
	map_metadata_.width = (int)size_x_/resolution_;   //Cells
	map_metadata_.height = (int)size_y_/resolution_; //Cells
	geometry_msgs::Pose orig;
	orig.position.x = origin_[0];
	orig.position.y = origin_[1];
	orig.position.z = 0.0;
	orig.orientation = tf::createQuaternionMsgFromYaw(origin_[2]);
	map_metadata_.origin = orig; //m

	dtMutex_.lock();
	distance_transform_ = cv::Mat(map_image_.rows,map_image_.cols,CV_32FC1);
	//cv::distanceTransform(map_image_,distance_transform_,CV_DIST_L1,3);
	dtMutex_.unlock();

}

void features::NavFeatures::goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	//printf("NavFeatures. Receiving goal!\n");
	setGoal(*msg);
}

/*void features::NavFeatures::goalAstarCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	setGoal(*msg);
}*/

void features::NavFeatures::peopleCallback(const upo_msgs::PersonPoseArrayUPO::ConstPtr& msg)
{
	//boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
	//printf("NavFeatures. Receiving people!\n");
	peopleMutex_.lock();
	people_ = msg->personPoses;
	if(people_.size() > 0) {
		people_frame_id_ = msg->header.frame_id;
	}
	peopleMutex_.unlock();

}

void features::NavFeatures::setPeople(upo_msgs::PersonPoseArrayUPO p)
{
	//printf("NavFeatures. Setting People!\n");
	peopleMutex_.lock();

    people_.clear();
    people_ = p.personPoses;
    people_frame_id_ = p.header.frame_id;

	peopleMutex_.unlock();
}

//Point_cloud2 callback
// Only used if laser projection is employed
void features::NavFeatures::pc2Callback(const sensor_msgs::PointCloud2::ConstPtr& pc_in){

	setObstacles(*pc_in);
}

//Point_cloud callback
// Only used if laser projection is employed
void features::NavFeatures::pcCallback(const sensor_msgs::PointCloud::ConstPtr& pc_in){

	sensor_msgs::PointCloud2 pc2;
	bool ok = sensor_msgs::convertPointCloudToPointCloud2(*pc_in, pc2);
	if(!ok) {
		ROS_WARN("NavFeatures. Error transforming pointCloud to pointCloud2");
	}

	setObstacles(pc2);

}

void features::NavFeatures::updateDistTransform(){

	sensor_msgs::PointCloud temp_pt_cloud;
	laserMutex_.lock();
	sensor_msgs::PointCloud2 lcloud = laser_cloud_;
	laserMutex_.unlock();

	if(lcloud.data.size() <= 0) {
		ROS_WARN("No cloud updated");
		return;
	}

	bool done = sensor_msgs::convertPointCloud2ToPointCloud(lcloud, temp_pt_cloud);
	if(!done)
		ROS_ERROR("\n\nNAV FEATURES. UPDATEDT. convertPointCloud2toPoingCloud!!!!!\n");

	//Add the laser readings (pointcloud) to the map
	dtMutex_.lock();
	cv::Mat map_copy = map_image_.clone();
	dtMutex_.unlock();
	for (int i =0;i<temp_pt_cloud.points.size();i++) {
		vector<int> pix;
		if(use_global_map_)
			pix = worldToMap(&(temp_pt_cloud.points[i]),&map_metadata_);
		else
			pix = BaseLinkWorldToImg(&(temp_pt_cloud.points[i]));

		if(pix[0] >= 0.0 && pix[0] < map_metadata_.width && pix[1] >= 0.0 && pix[1] < map_metadata_.height)
			map_copy.at<unsigned char>(pix[1],pix[0]) = 0;
		//else
			//ROS_WARN("\n\nNAV FEATURES. UPDATEDT. pixel out of range!\n");
	}

	//Remove the people detected from the map
	peopleMutex_.lock();
	vector<upo_msgs::PersonPoseUPO> per = people_;
	peopleMutex_.unlock();
	for (int i=0; i<per.size(); i++){
		geometry_msgs::Point32 temp_point;
		temp_point.x = per[i].position.x;
		temp_point.y = per[i].position.y;
		vector<int> pix;
		if(use_global_map_)
			pix = worldToMap(&temp_point,&map_metadata_);
		else
			pix = BaseLinkWorldToImg(&temp_point);

		if (pix[0] >= 0.0 && pix[0] < map_metadata_.width && pix[1] >= 0.0 && pix[1] < map_metadata_.height)
		{
			for (int j = -floor(people_paint_area_/2);j<ceil(people_paint_area_/2);j++){
				for (int k = -floor(people_paint_area_/2);k<ceil(people_paint_area_/2);k++){
					if (pix[1]+k>=0 && pix[0]+j>=0){
						map_copy.at<unsigned char>(pix[1]+k,pix[0]+j) = 255;
					}
				}

			}
		}
	}

	cv::Mat dt;
	try{
		//cv::distanceTransform(map_copy, distance_transform_, CV_DIST_L1,3);
		cv::distanceTransform(map_copy, dt, CV_DIST_L1, 3);
	} catch(...){
		ROS_ERROR("\n\nNAV FEATURES. UPDATEDT. cv::distanceTransform!!!!!\n");
	}

	//boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);

	dtMutex_.lock();
	distance_transform_ = dt.clone();
	dtMutex_.unlock();

	//gettimeofday(&stop, NULL);
	//printf("took %lu\n", stop.tv_usec - start.tv_usec);
	//imwrite(ros::package::getPath("navigation_features").append("/test_write.jpg"), map_copy);
}

vector<int> features::NavFeatures::worldToMap(geometry_msgs::Point32* world_point,nav_msgs::MapMetaData* map_metadata){
	vector<int> pixels;
	float x_map = world_point->x - map_metadata->origin.position.x;
	float y_map = world_point->y - map_metadata->origin.position.y;
	pixels.push_back((int)floor(x_map/map_metadata->resolution ));
	pixels.push_back((int)floor(y_map/map_metadata->resolution));
	return pixels;

}

/**
* transform point in base link frame (m) to pixel
*/
vector<int> features::NavFeatures::BaseLinkWorldToImg(geometry_msgs::Point32* point)
{
	vector<int> pix;
	float wx = origin_[0] + point->x;
	float wy = origin_[1] + point->y; //-
	pix.push_back((int)floor(wx/resolution_));
	pix.push_back((int)floor(wy/resolution_));
	return pix;

}

void features::NavFeatures::setGoal(geometry_msgs::PoseStamped g) {

	//boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
	//printf("NavFeatures. Receiving goal point!\n");
	goal_ = g;
}

void features::NavFeatures::setWeights(vector<float> we) {
	printf("NavFeatures. Setting weights: \n");
	w_.clear();
	for(unsigned int i=0; i<we.size(); i++)
	{
		//if(we[i] != 0.0) {
			w_.push_back(we[i]);
			//printf("we%u: %.3f, w_%u: %.3f\n", (i+1), we[i], (i+1), w_[i]);
		//}
	}

	for(unsigned int i=0; i<w_.size(); i++)
	{
		printf("w_%u: %.3f\n", (i+1), w_[i]);
	}

	/*if(w_.size() == 3)
		upo_featureset_ = 0;
	else if(w_.size() == 4)
		upo_featureset_ = 2;
	else
		upo_featureset_ = 1;
	*/
}

bool features::NavFeatures::poseValid(geometry_msgs::PoseStamped* pose)
{

	//Transform the coordinates
	vector<int> pix;
	geometry_msgs::PoseStamped sm;
	if(use_global_map_) {
		sm = transformPoseTo(*pose, string("/map"), false);
		geometry_msgs::Point32 point;
		point.x = sm.pose.position.x;
		point.y = sm.pose.position.y;
		point.z = 0.0;
		pix = worldToMap(&point, &map_metadata_);
	}else {
		sm = transformPoseTo(*pose, string("/base_link"), false);
		geometry_msgs::Point32 point;
		point.x = sm.pose.position.x;
		point.y = sm.pose.position.y;
		point.z = 0.0;
		pix = BaseLinkWorldToImg(&point);
	}
	//Distances to origin
	//float distance_x = sm.pose.position.x - (origin_)[0];
	//float distance_y = sm.pose.position.y - (origin_)[1];
	//float px = floor(distance_x/resolution_);
	//float py =floor(distance_y/resolution_);

	float px = pix[0];
	float py = pix[1];
	float distance = 0.0;
	dtMutex_.lock();
	if (py<0 || px<0  || px > map_image_.cols || py > map_image_.rows) {
		distance = 0.0;
	} else{
		try{

			distance = distance_transform_.at<float>(py,px)*resolution_;

		} catch(...)
		{
			ROS_ERROR("ERROR. POSE_VALID. px:%.2f, py:%.2f, distance:%.2f", px, py, distance);
			distance = 0.0;
		}
	}
	dtMutex_.unlock();
	// Take into account the robot radius
	if(distance <= insc_radius_robot_)
		return false;
	else
		return true;

}

//calculate the cost of a ray-traced line
/*float features::NavFeatures::lineCost(int x0, int x1, int y0, int y1){

	float line_cost = 0.0;
	float point_cost = -1.0;

	for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() )
	{
		point_cost = pointCost( line.getX(), line.getY() ); //Score the current point

		if(point_cost < 0)
			return -1;

		if(line_cost < point_cost)
			line_cost = point_cost;
	}
	return line_cost;
}*/

/*float features::NavFeatures::pointCost(int x, int y){
	unsigned char cost = costmap_local_->getCost(x, y);
	//if the cell is in an obstacle the path is invalid
	if(cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::NO_INFORMATION){
		return -1.0;
	}
		return cost;
}*/

float features::NavFeatures::getCost(geometry_msgs::PoseStamped* s)
{
	//boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);

	//configuration_mutex_.lock();

	vector<float> features;

	//if(upo_featureset_ != 2) {
		//Goal distance cost
		float dist_cost = goalDistFeature(s);

		if(dist_cost > 1.0) {
			//printf("GetCost. Dist_cost out of range > 1. returning 1!!!\n");
			dist_cost = 1.0;
		}
		if(dist_cost < 0.0){
			printf("GetCost. Dist_cost out of range < 0. Returning 0!!!\n");
			dist_cost = 0.0;
		}
		features.push_back(dist_cost);
	//}

	//Obstacle distance cost
	float obs_cost = obstacleDistFeature(s);
	features.push_back(obs_cost);
	if(obs_cost > 1.0 || obs_cost < 0.0) {
		printf("GetCost. Obs_cost out of range!!! = %.2f\n", obs_cost);
	}

	boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);

	if(upo_featureset_ == 0) {

		//Proxemics cost
		float prox_cost = proxemicsFeature(s);
		if(prox_cost > 1.0 || prox_cost < 0.0) {
			printf("GetCost. Prox_cost out of range!!!\n");
			prox_cost = 0.0;
		}
		features.push_back(prox_cost);

		//printf("GoalCost: %.3f, ObsCost: %.3f, ProxCost: %.3f\n", dist_cost, obs_cost, prox_cost);
		if(obs_cost == 1.0)
			return 1.0;

		//if(prox_cost == 0.0 && obs_cost != 0.0)
		//	return (0.5*dist_cost + 0.5*obs_cost);

	} else {  //split the proxemics cost (featureset 1 and 2)
		vector<float> prox_costs = gaussianFeatures(s);
		for(unsigned int i=0; i<prox_costs.size(); i++)
			features.push_back(prox_costs[i]);

		if(obs_cost == 1.0)
			return 1.0;
	}

	//configuration_mutex_.unlock();

	//if(prox_cost == 0.0 && obs_cost != 0.0)
	//	return (0.5*dist_cost + 0.5*obs_cost);
	//if(obs_cost == 0.0 && prox_cost != 0.0)
	//	return (0.5*prox_cost + 0.5*dist_cost);
	//if(prox_cost == 0.0 && obs_cost == 0.0)
	//	return dist_cost;

	float cost = 0.0;
	for(unsigned int i=0; i<w_.size(); i++)
		cost += w_[i]*features[i];

	//This is used for learning algorithms (MMP and RTL)
	loss_mutex_.lock();
	if(use_loss_func_ && !demo_path_.empty())
	{
		float l = calculate_loss_function(s);
		l = (float)(1-l); // /2.0;
		cost += l;

		//Two options:
		// 1. Saturate value
		//if(cost > 1.0)
		//	cost = 1.0;
		// 2. Normalize
		cost = cost/2.0;

	}
	loss_mutex_.unlock();
	return cost;
}

vector<float> features::NavFeatures::getFeatures(geometry_msgs::PoseStamped* s)
{
	boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);

	//configuration_mutex_.lock();

	vector<float> features;

	//if(upo_featureset_ != 2) { //featureset 2 is for A*, so we don't need this feature
		//Goal distance cost
		float dist_cost = goalDistFeature(s);
		features.push_back(dist_cost);
	//}
	//Obstacle distance cost
	float obs_cost = obstacleDistFeature(s);
	features.push_back(obs_cost);

	if(upo_featureset_ == 0) {
		//Proxemics cost
		float prox_cost = proxemicsFeature(s);
		features.push_back(prox_cost);

	} else { //split the proxemics cost (featureset 1 and 2)
		vector<float> prox_costs = gaussianFeatures(s);
		for(unsigned int i=0; i<prox_costs.size(); i++) {
			features.push_back(prox_costs[i]);
		}

	}

	//configuration_mutex_.unlock();

	return features;
}

vector<float> features::NavFeatures::getPathFeatureCount(vector<geometry_msgs::PoseStamped>* path)
{
	printf("NavFeatures. Request of path feature count...\n");
	vector<float> feature_counts;

	for(int i=0; i<path->size()-1; i++)
	{
		//We have to transform the coordinates to robot frame (/base_link)
		geometry_msgs::PoseStamped robot_pose = path->at(i);
		if(robot_pose.header.frame_id.compare("/base_link") != 0 && robot_pose.header.frame_id.compare("base_link") != 0)
			robot_pose = transformPoseTo(robot_pose, "/base_link", false);

		geometry_msgs::PoseStamped robot_pose2 = path->at(i+1);
		if(robot_pose2.header.frame_id.compare("/base_link") != 0 && robot_pose2.header.frame_id.compare("base_link") != 0)
			robot_pose2 = transformPoseTo(robot_pose2, "/base_link", false);

		double dx = robot_pose.pose.position.x - robot_pose2.pose.position.x;
		double dy = robot_pose.pose.position.y - robot_pose2.pose.position.y;
		double d = hypot(dx,dy);

		if(!isQuaternionValid(robot_pose.pose.orientation)) {
			robot_pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
			printf("¡¡¡¡¡¡QUATERNION NO VALID!!!!!. Changing yaw to zero.\n");
		}

		update();

		vector<float> features1 = getFeatures(&robot_pose);
		vector<float> features2 = getFeatures(&robot_pose2);

		if(i==0)
			feature_counts.assign(features1.size(), 0);

		for(unsigned int j=0; j<features1.size(); j++)
		{
			feature_counts.at(j) = feature_counts.at(j) + (features1.at(j) + features2.at(j))*d/2.0;
		}

	  }
	  return feature_counts;
}

float features::NavFeatures::goalDistFeature(geometry_msgs::PoseStamped* s)  {

	//if we don't have a goal yet
	if(goal_.header.frame_id.empty())
			return (float) 0.5;

	//if the frame_id are different, transform s to goal frame
	geometry_msgs::PoseStamped p = transformPoseTo(*s, goal_.header.frame_id, false);
	float dx = goal_.pose.position.x - p.pose.position.x;
	float dy = goal_.pose.position.y - p.pose.position.y;
	float dist = (float)(sqrt(dx*dx + dy*dy)/max_planning_dist_);
	if(dist > 1.0) {
		//printf("NavFeatures. goal dist feature %.2f > 1.0. d: %.2f, max_dist: %.2f\n", dist, (float)sqrt(dx*dx + dy*dy), max_planning_dist_);
		dist = 1.0;
	}
	return dist;
}

float features::NavFeatures::obstacleDistFeature(geometry_msgs::PoseStamped* s)  {

	boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
	//configuration_mutex_.lock();

	//Transform the coordinates
	vector<int> pix;
	geometry_msgs::PoseStamped sm;
	if(use_global_map_) {
		sm = transformPoseTo(*s, string("/map"), false);
		geometry_msgs::Point32 point;
		point.x = sm.pose.position.x;
		point.y = sm.pose.position.y;
		point.z = 0.0;
		pix = worldToMap(&point, &map_metadata_);
	}else {
		sm = transformPoseTo(*s, string("/base_link"), false);
		geometry_msgs::Point32 point;
		point.x = sm.pose.position.x;
		point.y = sm.pose.position.y;
		point.z = 0.0;
		pix = BaseLinkWorldToImg(&point);
	}

	float px = pix[0];
	float py = pix[1];

	// This functions transforms position of people to pixels on map, gets distance and then converts to meters
	//float distance_x = sm.pose.position.x - (origin_)[0];
	//float distance_y = sm.pose.position.y - (origin_)[1];

	//float px = floor(distance_x/resolution_);
	//float py =distanceTransform->rows-floor(distance_y /resolution);
	//float py =floor(distance_y/resolution_);
	//cout<<"px:"<<px<<"  py:"<<py<<endl;
	float distance = 0.0;
	dtMutex_.lock();
	if (py<0 || px<0  || px > map_image_.cols || py > map_image_.rows) {
		distance = 0.0;
	} else{
		try{

			distance = distance_transform_.at<float>(py,px)*resolution_;

		} catch(...)
		{
			ROS_ERROR("ERROR. OBSTACLE FEATURE. px:%.2f, py:%.2f, distance:%.2f", px, py, distance);
			distance = 0.0;
		}
	}
	dtMutex_.unlock();
	//cout << "after distance_trans" << endl;
	// Take into account the robot radius
	if(distance <= insc_radius_robot_)
		distance = 0.0;
	else
		distance = distance - insc_radius_robot_;

	float d = distance_functions(distance, INVERSE_DEC); //EXP_DEC //INVERSE_DEC
	//Normalize
	//printf("Distance: %.4f, Obs cost: %.4f, norm: %.4f\n", distance, d, (d/max_cost_obs_));
	d = d/max_cost_obs_;
	if(d > 1.0) {
		printf("NavFeatures. obstacle dist feature %.2f > 1.0\n", d);
		d = 1.0;
	}

	//configuration_mutex_.unlock();

	return d;

}

float features::NavFeatures::distance_functions(const float distance, const dist_type type){
	// different distance transformations between two points are defined here
	// _INC means the function is monotonically increasing
	// _DEC means that the function is monotonically decreasing
	float out=0;
	switch (type){
		case LINEAR_INC:
			out = distance;
			break;
		case LOG_INC:
			out = 2*log(1+distance);
			break;
		case EXP_INC:
			out = exp((distance-2)/4-1);
			break;
		case INVERSE_DEC:
			out = 2/(distance+0.2);
			break;
		case LOG_DEC:
			out = 1/log(0.5*distance+1.2);
			break;
		case EXP_DEC:
			out = exp(3-3*distance);
			break;
		default:
			out=distance;
		break;
	}
	return out;
}

void features::NavFeatures::calculateGaussians()
{

	vector<upo_msgs::PersonPoseUPO> people_aux;
	string frame;
	//read the person list (thread safe)
	peopleMutex_.lock();
	people_aux = people_;
	frame = people_frame_id_;
	peopleMutex_.unlock();

	//printf("Frame %s\n", frame.c_str());
	//printf("\n\ncalculateGaussians. People detected: %u\n\n", (unsigned int)people_aux.size());

	vector<gaussian> gauss_aux;

	if(grouping_)
	{

		vector<group_candidate> group_candidates;
		for(unsigned int i=0; i<people_aux.size(); i++)
		{
			//If the person is moving, we  do not group for the moment
			if(people_aux.at(i).vel > 0.18) {

				gaussian f;
				f.type = FRONT;
				f.x = people_aux.at(i).position.x;
				f.y = people_aux.at(i).position.y;
				f.th = tf::getYaw(people_aux.at(i).orientation);
				f.sx = sigmas_[0]; // 1.2
				f.sy = sigmas_[1]; // 1.2/1.5 = 0.8

				if(people_aux.at(i).id == it_id_)
				{
					f.type = FRONT_APPROACH;
				}
				gauss_aux.push_back(f);

				gaussian b;
				b.type = BACK;
				b.x = people_aux.at(i).position.x;
				b.y = people_aux.at(i).position.y;
				float angle = (tf::getYaw(people_aux.at(i).orientation)) + M_PI;
				b.th = normalizeAngle(angle, -M_PI, M_PI);
				b.sx = sigmas_[2]; // 0.8
				b.sy = sigmas_[3]; // 0.8

				gauss_aux.push_back(b);

			} else {  //Obtain the group candidates

				group_candidate cand;
				geometry_msgs::Pose2D p1;
				p1.x = people_aux.at(i).position.x;
				p1.y = people_aux.at(i).position.y;
				p1.theta = tf::getYaw(people_aux.at(i).orientation);
				cand.position = p1;
				geometry_msgs::Pose2D p2;
				//Calculate the iteraction points
				p2.x = p1.x + (grouping_distance_/2.0)*cos(p1.theta);
				p2.y = p1.y + (grouping_distance_/2.0)*sin(p1.theta);
				p2.theta = p1.theta;
				cand.interaction_point = p2;
				cand.group_id = -1;
				cand.id = people_aux.at(i).id;
				group_candidates.push_back(cand);
			}
		}

		//check distance between the interaction points
		vector<group_candidate> group;
		int group_number = 1;
		for(unsigned int i=0; i<group_candidates.size(); i++)
		{
			if(group_candidates.at(i).group_id == -1) {
				group_candidates.at(i).group_id = group_number;
				group.push_back(group_candidates.at(i));

				geometry_msgs::Pose2D c1 = group_candidates.at(i).interaction_point;
				for(unsigned int j=0; j<group_candidates.size(); j++)
				{
					if(j!=i && group_candidates.at(j).group_id == -1) {
						geometry_msgs::Pose2D c2 = group_candidates.at(j).interaction_point;
						float dist = sqrt((c1.x-c2.x)*(c1.x-c2.x) + (c1.y-c2.y)*(c1.y-c2.y));
						if(dist < ((grouping_distance_/2.0)*0.9))
						{
							group_candidates.at(j).group_id = group_number;
							group.push_back(group_candidates.at(j));
						}
					}
				}

				//Check the members of the group and create the
				//corresponding gaussians
				if(group.size() > 0)
				{
					//Only one person --> create the regular gaussian function
					if(group.size() == 1)
					{
						gaussian f;
						f.type = FRONT;
						f.x = group.at(0).position.x;
						f.y = group.at(0).position.y;
						f.th = group.at(0).position.theta;
						f.sx = sigmas_[0]; // 1.2
						f.sy = sigmas_[1]; // 1.2/1.5 = 0.8

						if(group.at(0).id == it_id_)
							f.type = FRONT_APPROACH;

						gauss_aux.push_back(f);

						gaussian b;
						b.type = BACK;
						b.x = group.at(0).position.x;
						b.y = group.at(0).position.y;
						float angle = (group.at(0).position.theta) + M_PI;
						b.th = normalizeAngle(angle, -M_PI, M_PI);
						b.sx = sigmas_[2]; // 0.8
						b.sy = sigmas_[3]; // 0.8

						gauss_aux.push_back(b);

					} else {
						// Calculate the center of the group
						float x = 0.0;
						float y = 0.0;
						float th = 0.0;
						float max_dist = 0.0;
						for(unsigned int k=0; k<group.size(); k++)
						{
							x = x + group.at(k).interaction_point.x;
							y = y + group.at(k).interaction_point.y;
							th = th + group.at(k).interaction_point.theta;

							float xk = group.at(k).position.x;
							float yk = group.at(k).position.y;
							for(unsigned int h=0; h<group.size() && h!=k; h++)
							{
								float xh = group.at(h).position.x;
								float yh = group.at(h).position.y;
								float d = sqrt((xk-xh)*(xk-xh) + (yk-yh)*(yk-yh));
								if(d > max_dist)
									max_dist = d;
							}
						}

						bool ap = false;
						for(unsigned int r=0; r<group.size() && (ap==false); r++)
						{
							if(group.at(r).id == it_id_)
								ap = true;
						}
						th = normalizeAngle(th, -M_PI, M_PI);
						th = th/group.size();
						th = th + M_PI;
						th = normalizeAngle(th, -M_PI, M_PI);

						x = x/group.size();
						y = y/group.size();

						gaussian g;
						if(ap)
							g.type = AROUND_APPROACH;
						else
							g.type = AROUND;
						g.x = x;
						g.y = y;
						g.th = th;
						g.sx = sigmas_[2] + max_dist/2.0; // 0.8
						g.sy = sigmas_[2] + max_dist/2.0;  // 0.8
						gauss_aux.push_back(g);
					}
				}
				group.clear();
				group_number++;
			}
		}

	} else {

		for(unsigned int i=0; i<people_aux.size(); i++)
		{
			if(people_aux.at(i).id == it_id_ && it_remove_gauss_)
			{
				continue;
			}

			gaussian f;
			f.type = FRONT;
			f.x = people_aux.at(i).position.x;
			f.y = people_aux.at(i).position.y;
			f.th = tf::getYaw(people_aux.at(i).orientation);
			f.sx = sigmas_[0]; // 1.2
			f.sy = sigmas_[1]; // 1.2/1.5 = 0.8

			if(people_aux.at(i).id == it_id_)
			{
				f.type = FRONT_APPROACH;
			}

			gauss_aux.push_back(f);

			gaussian b;
			b.type = BACK;
			b.x = people_aux.at(i).position.x;
			b.y = people_aux.at(i).position.y;
			float angle = (tf::getYaw(people_aux.at(i).orientation)) + M_PI;
			b.th = normalizeAngle(angle, -M_PI, M_PI);
			b.sx = sigmas_[2]; // 0.8
			b.sy = sigmas_[3]; // 0.8

			gauss_aux.push_back(b);

			if(upo_featureset_ == 1 || upo_featureset_ == 2) {

				gaussian r;
				r.type = RIGHT;
				r.x = people_aux.at(i).position.x;
				r.y = people_aux.at(i).position.y;
				float angle = (tf::getYaw(people_aux.at(i).orientation)) - M_PI/2.0;
				r.th = normalizeAngle(angle, -M_PI, M_PI);
				r.sx = sigmas_[4]; // 0.8
				r.sy = sigmas_[5]; // 0.4

				gauss_aux.push_back(r);
			}

		}
	}

	gaussianMutex_.lock();
	gaussians_ = gauss_aux;
	gaussianMutex_.unlock();

	visualization_msgs::MarkerArray m;
	vector<visualization_msgs::Marker> markers;
	visualization_msgs::Marker marker;
	for(unsigned int i=0; i<gauss_aux.size(); i++)
	{
		marker.header.frame_id = frame;
		marker.header.stamp = ros::Time::now();
		marker.ns = "gaussians";
		marker.id = (i+1);
		marker.type = visualization_msgs::Marker::ARROW;
		marker.action = visualization_msgs::Marker::ADD;
		marker.pose.position.x = gauss_aux.at(i).x;
		marker.pose.position.y = gauss_aux.at(i).y;
		marker.pose.position.z = 1.1;
		marker.pose.orientation = tf::createQuaternionMsgFromYaw(gauss_aux.at(i).th);
		marker.scale.x = 0.7;
		marker.scale.y = 0.15;
		marker.scale.z = 0.15;
		marker.color.r = 0.0f;
		marker.color.g = 0.0f;
		marker.color.b = 1.0f;
		marker.color.a = 1.0;
		marker.lifetime = ros::Duration(1.0);
		m.markers.push_back(marker);
	}
	if(gauss_aux.size() > 0)
		pub_gaussian_markers_.publish(m);

	//printf("\n\n %u Gaussians calculated!!!!\n", (unsigned int)gauss_aux.size());
}

void features::NavFeatures::update()
{
	boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
	calculateGaussians();
	updateDistTransform();

}

float features::NavFeatures::proxemicsFeature(geometry_msgs::PoseStamped* s)  {

	vector<gaussian> gauss;
	gaussianMutex_.lock();
	gauss = gaussians_;
	gaussianMutex_.unlock();

	if(gauss.size() == 0) {
		//printf("gaussian size = 0. Returning 0.0\n");
		return 0.0;
	}

	peopleMutex_.lock();
	string people_frame = people_frame_id_;
	peopleMutex_.unlock();

	//We need to transform the sample to the same frame of the people
	geometry_msgs::PoseStamped robot = *s;
	robot = transformPoseTo(robot, people_frame, false); //true

	if(robot.header.frame_id.empty()){
		printf("frame id empty. returning 0\n");
		return 0.0;
	}
	float rx = robot.pose.position.x;
	float ry = robot.pose.position.y;

	float prox_cost = 1.0;
	for (vector<gaussian>::iterator it = gauss.begin(); it != gauss.end(); ++it)
	{
		//odom coordinates
		float ph = it->th;
		float px = it->x;
		float py = it->y;

		/*
		First, transform the robot location into gaussian location frame:
									|cos(th)  sin(th)  0|
			Rotation matrix R(th)= 	|-sin(th) cos(th)  0|
									|  0        0      1|

			x' = (xr-xp)*cos(th_p)+(yr-yp)*sin(th_p)
			y' = (xr-xp)*(-sin(th_p))+(yr-yp)*cos(th_p)
		*/
		float x_r = (rx - px)*cos(ph) + (ry - py)*sin(ph);
		float y_r = (rx - px)*(-sin(ph)) + (ry - py)*cos(ph);

		//printf("robot x:%.2f, y:%.2f, sx:%.2f, sy:%.2f\n", x_r, y_r, it->sx, it->sy);

		float cost = 0.0;

		if(it->type == FRONT_APPROACH || it->type == AROUND_APPROACH)
		{
			float angle = atan2(y_r, x_r);
			if(angle > (-approaching_angle_) && angle < approaching_angle_ && x_r > 0.8) {
				prox_cost = 0.0;
				return prox_cost;
			} else
				cost = gaussian_function(x_r, y_r, it->sx, it->sy);
		}
		if(it->type == AROUND) {
			cost = gaussian_function(x_r, y_r, it->sx, it->sy);
		} else if (it->type == FRONT) {
			if(x_r >= 0.0) {
				cost = gaussian_function(x_r, y_r, it->sx, it->sy);
			}
		} else {  //type BACK
			if(x_r > 0.0) {
				cost = gaussian_function(x_r, y_r, it->sx, it->sy);
			}
		}

		//prox_cost = prox_cost * (1.0 + cost);
		if((1.0+cost) > prox_cost)
			prox_cost = 1.0+cost;

	}

	prox_cost = prox_cost - 1.0;
	//printf("prox_cost: %.3f\n", prox_cost);

	//return (prox_cost<=1.0)?prox_cost:1.0;
	if(prox_cost > 1.0) {
		//printf("prox_cost = %.3f\n", prox_cost);
		prox_cost = 1.0;
	}
	//printf("prox_cost: %.3f\n", prox_cost);
	return prox_cost;
}

vector<float> features::NavFeatures::gaussianFeatures(geometry_msgs::PoseStamped* s)
{
	vector<gaussian> gauss;
	gaussianMutex_.lock();
	gauss = gaussians_;
	gaussianMutex_.unlock();

	vector<float> pcosts;
	pcosts.push_back(0.0); //Front
	pcosts.push_back(0.0); //Back
	pcosts.push_back(0.0); //Right side

	//Remember not to group people to use this feature set
	if(gauss.size() == 0) {
		return pcosts;
	}

	peopleMutex_.lock();
	string people_frame = people_frame_id_;
	peopleMutex_.unlock();

	//We need to transform the sample to the same frame of the people
	geometry_msgs::PoseStamped robot = *s;
	robot = transformPoseTo(robot, people_frame, false); //true

	if(robot.header.frame_id.empty()){
		printf("frame id empty. returning 0\n");
		return pcosts;
	}
	float rx = robot.pose.position.x;
	float ry = robot.pose.position.y;

	for (vector<gaussian>::iterator it = gauss.begin(); it != gauss.end(); ++it)
	{
		//odom coordinates
		float ph = it->th;
		float px = it->x;
		float py = it->y;

		/*
		First, transform the robot location into gaussian location frame:
									|cos(th)  sin(th)  0|
			Rotation matrix R(th)= 	|-sin(th) cos(th)  0|
									|  0        0      1|

			x' = (xr-xp)*cos(th_p)+(yr-yp)*sin(th_p)
			y' = (xr-xp)*(-sin(th_p))+(yr-yp)*cos(th_p)
		*/
		float x_r = (rx - px)*cos(ph) + (ry - py)*sin(ph);
		float y_r = (rx - px)*(-sin(ph)) + (ry - py)*cos(ph);

		//printf("robot x:%.2f, y:%.2f, sx:%.2f, sy:%.2f\n", x_r, y_r, it->sx, it->sy);

		if (it->type == FRONT) {
			if(x_r >= 0.0) {
				float cost = gaussian_function(x_r, y_r, it->sx, it->sy);
				if(cost > pcosts[0])
					pcosts[0] = cost;
			}
		} else if(it->type == BACK){
			if(x_r > 0.0) {
				float cost = gaussian_function(x_r, y_r, it->sx, it->sy);
				if(cost > pcosts[1])
					pcosts[1] = cost;
			}
		} else if(it->type == RIGHT) {
			if(x_r > 0.0) {
				float cost = gaussian_function(x_r, y_r, it->sx, it->sy);
				if(cost > pcosts[2])
					pcosts[2] = cost;
			}
		}

	}

	return pcosts;
}

/*
float features::NavFeatures::proxemicsFeature(geometry_msgs::PoseStamped* s)  {

	callbackMutex_.lock();
	unsigned int size = (unsigned int)people_.size();
	callbackMutex_.unlock();

	if(size == 0)
		return 0.0;

	//We need to transform the state sample to the same people frame
	geometry_msgs::PoseStamped robot = *s;

	callbackMutex_.lock();
	robot = transformPoseTo(robot, people_frame_id_, true);
	callbackMutex_.unlock();

	float prox_cost = getProxemicsCost((float)robot.pose.position.x, (float)robot.pose.position.y);
	//return prox_cost/amp_;
	return prox_cost;
}*/

/*float features::NavFeatures::getProxemicsCost(float rx, float ry)  {

	// read the person list (thread safe)
	vector<upo_msgs::PersonPoseUPO> people_aux;
	peopleMutex_.lock();
	people_aux = people_;
	peopleMutex_.unlock();

	// Cost 0.0 if no persons detected
	if(people_aux.size() == 0)
	{
		return 0.0;
  	}

	float prox_cost = 1;

  	for (vector<upo_msgs::PersonPoseUPO>::iterator it = people_aux.begin(); it != people_aux.end(); ++it)
  	{
		//odom coordinates
		float ph = tf::getYaw(it->orientation);
		float px = it->position.x;
		float py = it->position.y;
		bool front = true;*/
		/*
		First, transform the robot location into person location frame:
									|cos(th)  sin(th)  0|
			Rotation matrix R(th)= 	|-sin(th) cos(th)  0|
									|  0        0      1|

			x' = (xr-xp)*cos(th_p)+(yr-yp)*sin(th_p)
			y' = (xr-xp)*(-sin(th_p))+(yr-yp)*cos(th_p)
		*/
		/*float x_r = (rx - px)*cos(ph) + (ry - py)*sin(ph);
		float y_r = (rx - px)*(-sin(ph)) + (ry - py)*cos(ph);
		if(x_r < 0.0)
			front = false;

		float cost = gaussian_function(x_r, y_r, front);

		prox_cost = prox_cost * (cost + 1.0);

	}

	prox_cost = prox_cost - 1.0;

	return (prox_cost<=1.0)?prox_cost:1.0;

}*/

float features::NavFeatures::gaussian_function(float x, float y, float sx, float sy)
{
	float g = (amp_*exp(-( (x*x)/(2*(sx*sx)) + (y*y)/(2*(sy*sy))))) / amp_;
	if(g > 1.0) {
		printf("NavFeatures. gaussian value %.2f > 1.0\n", g);
		g = 1.0;
	}
	return g;
}

float features::NavFeatures::gaussian_function(float x, float y, bool fr)
{
	float sx, sy;
	//front
	if(fr) {
		sx = sigmas_[0]; // 1.2
		sy = sigmas_[1]; // 1.2/1.5 = 0.8
	} else { //back
		sx = sigmas_[2]; // 0.8
		sy = sigmas_[3]; // 0.8
	}
	return (amp_*exp(-( (x*x)/(2*(sx*sx)) + (y*y)/(2*(sy*sy))))) / amp_;

	//phi in radians
	//positions are in odom coordinates
	//Proxemics:
	// intimate: 15-45 cm
	// personal: 46-120 cm
	// social: 120-360 cm
	// public: >360 cm
	/*double A = amp_;
	double x = x_rob;
	double y = y_rob;
	double x0 = x_per;
	double y0 = y_per;
	double z = 0.0;

	// 3sigma = 3.60 --> sigma = 1.20
	// 2sigma = 3.60 --> sigma = 1.28
	double sigmaX = sigmas.at(0); //front 1.20
	//Según el artículo "Companion..." de Kirby --> sigmaX = 1.5*sigmaY;
	double sigmaY = sigmas.at(1);  //aside 1.20/1.50 = 0.8

	// front
	if(fr) {
	  double a = pow(cos(phi),2)/2/pow(sigmaX,2) + pow(sin(phi),2)/2/pow(sigmaY,2);
	  double b = - sin(2*phi)/4/pow(sigmaX,2) + sin(2*phi)/4/pow(sigmaY,2);
	  double c = pow(sin(phi),2)/2/pow(sigmaX,2) + pow(cos(phi),2)/2/pow(sigmaY,2);
	  z = A*exp(-(a*pow((x-x0),2) + 2*b*(x-x0)*(y-y0) + c*pow((y-y0),2)));

	} else { //back
	  //take the opposite orientation
	  double o_phi = 0.0;
	  if(phi > 0)
	    o_phi = phi - M_PI;
	  else
	    o_phi = phi + M_PI;

	  sigmaX = sigmas.at(2);
	  sigmaY = sigmas.at(3);
	  double a = pow(cos(o_phi),2)/2/pow(sigmaX,2) + pow(sin(o_phi),2)/2/pow(sigmaY,2);
	  double b = - sin(2*o_phi)/4/pow(sigmaX,2) + sin(2*o_phi)/4/pow(sigmaY,2);
	  double c = pow(sin(o_phi),2)/2/pow(sigmaX,2) + pow(cos(o_phi),2)/2/pow(sigmaY,2);
	  z = A*exp(-(a*pow((x-x0),2) + 2*b*(x-x0)*(y-y0) + c*pow((y-y0),2)));
	}
	//ROS_INFO("GC=%.3f, xp=%.1f, yp=%.1f, h=%.1f, xr=%.1f, yr=%.1f, sx=%.1f sy=%.1f",z,x0,y0,phi,x,y,sigmaX,sigmaY);
	return z;*/
}

geometry_msgs::PoseStamped features::NavFeatures::transformPoseTo(geometry_msgs::PoseStamped pose_in, string frame_out, bool usetime)
{
	geometry_msgs::PoseStamped in = pose_in;
	if(!usetime)
		in.header.stamp = ros::Time();

	geometry_msgs::PoseStamped pose_out;

	geometry_msgs::Quaternion q = in.pose.orientation;
	if(!isQuaternionValid(q))
	{
		ROS_WARN("NavFeatures. transformPoseTo. Quaternion no valid. Creating new quaternion with yaw=0.0");
		in.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
	}
	try {
		tf_listener_->transformPose(frame_out.c_str(), in, pose_out);
	}catch (tf::TransformException ex){
		ROS_WARN("NavFeatures. TransformException in method transformPoseTo. TargetFrame: %s : %s", frame_out.c_str(), ex.what());
	}
	//printf("Tranform pose. frame_in: %s, x:%.2f, y:%.2f, frame_out: %s, x:%.2f, y:%.2f\n", in.header.frame_id.c_str(), in.pose.position.x, in.pose.position.y, frame_out.c_str(), pose_out.pose.position.x, pose_out.pose.position.y);
	return pose_out;
}

bool features::NavFeatures::isQuaternionValid(const geometry_msgs::Quaternion q){
    //first we need to check if the quaternion has nan's or infs
    if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
		ROS_ERROR("Quaternion has infs!!!!");
		return false;
    }
    if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w)) {
		ROS_ERROR("Quaternion has nans !!!");
		return false;
	}

	if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01) {
		ROS_ERROR("Quaternion malformed, magnitude: %.3f should be 1.0", (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w));
		return false;
	}

    tf::Quaternion tf_q(q.x, q.y, q.z, q.w);

    //next, we need to check if the length of the quaternion is close to zero
    if(tf_q.length2() < 1e-6){
      ROS_ERROR("Quaternion has length close to zero... discarding.");
      return false;
    }

    //next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
    tf_q.normalize();

    tf::Vector3 up(0, 0, 1);

    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

    if(fabs(dot - 1) > 1e-3){
      ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
      return false;
    }

    return true;
}

float features::NavFeatures::normalizeAngle(float val, float min, float max) {

	float norm = 0.0;
	if (val >= min)
		norm = min + fmod((val - min), (max-min));
	else
		norm = max - fmod((min - val), (max-min));

    return norm;
}


#pragma once

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/AssemblyValsCache.hpp>
#include <polyfem/ElasticityUtils.hpp>

#include <Eigen/Sparse>
#include <vector>
#include <iostream>
#include <cmath>
#include <memory>

//mass matrix assembler
namespace polyfem
{
	class MassMatrixAssembler
	{
	public:
		//assembles the mass matrix
		//mesh is volumetric
		//size of the problem (eg, 1 for laplace)
		//number of bases, bases and geom bases
		//density, class that can evaluate per point density
		void assemble(
			const bool is_volume,
			const int size,
			const int n_basis,
			const Density &density,
			const std::vector<ElementBases> &bases,
			const std::vector<ElementBases> &gbases,
			const AssemblyValsCache &cache,
			StiffnessMatrix &mass) const;
	};
} // namespace polyfem


//==================================================================================================
/*!
  @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_SCALAR_FUNCTION_FREXP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_FREXP_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/fast.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#endif
#include <boost/simd/constant/limitexponent.hpp>
#include <boost/simd/constant/mask1frexp.hpp>
#include <boost/simd/constant/mask2frexp.hpp>
#include <boost/simd/constant/maxexponentm1.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/bitwise_and.hpp>
#include <boost/simd/function/scalar/bitwise_andnot.hpp>
#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/simd/function/scalar/bitwise_or.hpp>
#include <boost/simd/function/scalar/is_invalid.hpp>
#include <boost/simd/function/scalar/shr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
 BOOST_DISPATCH_OVERLOAD ( frexp_
                         , (typename A0, typename A1)
                         , bd::cpu_
                         , bd::scalar_< bd::floating_<A0> >
                         , bd::scalar_< bd::floating_<A0> >
                         , bd::scalar_< bd::integer_<A1>  >
                         )
 {
    BOOST_FORCEINLINE void operator() ( A0 a0,A0 & r0,A1& r1) const BOOST_NOEXCEPT
    {
      if (a0 == 0 || is_invalid(a0))
      {
        r0 = a0;
        r1 = Zero<A1>();
      }
      else
      {
        using i_t = bd::as_integer_t<A0, signed>;
        r1 = simd::bitwise_cast<i_t>(bitwise_and(Mask1frexp<A0>(), a0));  // extract exp.
#ifndef BOOST_SIMD_NO_DENORMALS
        A1 t = Zero<A1>();
        if(is_eqz(r1)) // denormal
        {
          a0 *= Twotonmb<A0>();
          r1 = simd::bitwise_cast<i_t>(bitwise_and(Mask1frexp<A0>(), a0));  // extract exp. again
          t = Nbmantissabits<A0>();
        }
#endif
        A0 x  = bitwise_andnot(a0, Mask1frexp<A0>());                    // clear exp. in a0
        r1 = shr(r1,Nbmantissabits<A0>())- Maxexponentm1<A0>();         // compute exp.
        if (r1 > Limitexponent<A0>())
        {
          r1   = 0;
          r0   = a0;
          return;
        }
        r0 = bitwise_or(x,Mask2frexp<A0>());                                   // insert exp.+1 in x
#ifndef BOOST_SIMD_NO_DENORMALS
        r1 -= t;
#endif
      }
    }
  };

 BOOST_DISPATCH_OVERLOAD ( frexp_
                         , (typename A0, typename A1)
                         , bd::cpu_
                         , bd::scalar_< bd::floating_<A0> >
                         , bd::scalar_< bd::floating_<A0> >
                         , bd::scalar_< bd::integer_<A1>  >
                         , boost::simd::fast_tag
                         )
 {
    BOOST_FORCEINLINE void operator() ( A0 a0,A0 & r0,A1& r1
                                      , fast_tag const&) const BOOST_NOEXCEPT
    {
      r1  = bitwise_cast<A1>(bitwise_and(Mask1frexp<A0>(), a0));
      A0  x = bitwise_andnot(a0, Mask1frexp<A0>());
      r1  = shr(r1,Nbmantissabits<A0>()) - Maxexponentm1<A0>();
      r0  = bitwise_or(x,Mask2frexp<A0>());
    }
  };

 BOOST_DISPATCH_OVERLOAD ( frexp_
                         , (typename A0, typename A1)
                         , bd::cpu_
                         , bd::scalar_< bd::floating_<A0> >
                         , bd::scalar_< bd::integer_<A1>  >
                         , boost::simd::std_tag
                         )
 {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1& r1
                                      , std_tag const&) const BOOST_NOEXCEPT
    {
      int e = Zero<int>();
      A0 r = std::frexp(a0, &e);
      r1 = e;
      return r;
    }
  };

 BOOST_DISPATCH_OVERLOAD ( frexp_
                         , (typename A0, typename A1)
                         , bd::cpu_
                         , bd::scalar_< bd::floating_<A0> >
                         , bd::scalar_< bd::floating_<A0> >
                         , bd::scalar_< bd::integer_<A1>  >
                         , boost::simd::std_tag
                         )
 {
    BOOST_FORCEINLINE void operator() ( A0 a0, A0& r1, A1& r2
                                      , std_tag const&) const BOOST_NOEXCEPT
    {
      int e = Zero<int>();
      r1 = std::frexp(a0, &e);
      r2 = e;
    }
  };

 BOOST_DISPATCH_OVERLOAD ( frexp_
                         , (typename A0)
                         , bd::cpu_
                         , bd::scalar_< bd::floating_<A0> >
                         , boost::simd::std_tag
                         )
 {
   using i_t = bd::as_integer_t<A0, signed>;
   BOOST_FORCEINLINE std::pair < A0, i_t> operator() ( A0 a0
                                                     , std_tag const&) const BOOST_NOEXCEPT
    {
      int e = Zero<int>();
      A0 r1 = std::frexp(a0, &e);
      return {r1, i_t(e)};
    }
  };

} } }

#endif


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 14:37:45

#include "MSSMatMSUSYEFTHiggs_mAmu_two_scale_initial_guesser.hpp"
#include "MSSMatMSUSYEFTHiggs_mAmu_two_scale_model.hpp"
#include "MSSMatMSUSYEFTHiggs_mAmu_standard_model_two_scale_matching.hpp"
#include "standard_model_two_scale_model.hpp"
#include "lowe.h"
#include "error.hpp"
#include "ew_input.hpp"
#include "wrappers.hpp"

#include <Eigen/Core>

namespace flexiblesusy {

#define INPUTPARAMETER(p) model->get_input().p
#define MODELPARAMETER(p) model->get_##p()
#define SMPARAMETER(p) eft->get_##p()
#define PHASE(p) model->get_##p()
#define LowEnergyConstant(p) Electroweak_constants::p
#define MODEL model

MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser(
   MSSMatMSUSYEFTHiggs_mAmu<Two_scale>* model_,
   standard_model::StandardModel<Two_scale>* eft_,
   const softsusy::QedQcd& qedqcd_,
   const standard_model::Standard_model_low_scale_constraint<Two_scale>& low_constraint_,
   const MSSMatMSUSYEFTHiggs_mAmu_susy_scale_constraint<Two_scale>& susy_constraint_
)
   : Initial_guesser()
   , model(model_)
   , eft(eft_)
   , qedqcd(qedqcd_)
   , low_constraint(low_constraint_)
   , susy_constraint(susy_constraint_)
{
   if (!model)
      throw SetupError("MSSMatMSUSYEFTHiggs_mAmu_initial_guesser: Error: pointer to model"
                       " MSSMatMSUSYEFTHiggs_mAmu<Two_scale> must not be zero");
}

MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::~MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser()
{
}

/**
 * Guesses the DR-bar model parameters by calling
 * guess_susy_parameters() and guess_soft_parameters() .
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::guess()
{
   guess_eft_parameters();
   guess_model_parameters();
}

/**
 * Guesses the effective parameters (gauge, Yukawa couplings) at
 * \f$m_\text{top}^\text{pole}\f$ from the Standard Model gauge
 * couplings and fermion masses.  Threshold corrections are ignored.
 * The user-defined initial guess at the low-scale
 * (InitialGuessAtLowScale) are ignored
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::guess_eft_parameters()
{
   softsusy::QedQcd leAtMt(qedqcd);
   const double mtpole = leAtMt.displayPoleMt();

   mu_guess = leAtMt.displayMass(softsusy::mUp);
   mc_guess = leAtMt.displayMass(softsusy::mCharm);
   mt_guess = model->get_thresholds() > 0 && model->get_threshold_corrections().mt > 0 ?
      leAtMt.displayMass(softsusy::mTop) - 30.0 :
      leAtMt.displayPoleMt();
   md_guess = leAtMt.displayMass(softsusy::mDown);
   ms_guess = leAtMt.displayMass(softsusy::mStrange);
   mb_guess = leAtMt.displayMass(softsusy::mBottom);
   me_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(softsusy::mElectron) :
      leAtMt.displayPoleMel();
   mm_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(softsusy::mMuon) :
      leAtMt.displayPoleMmuon();
   mtau_guess = leAtMt.displayMass(softsusy::mTau);

   // guess gauge couplings at mt
   const auto alpha_sm(leAtMt.guess_alpha_SM5(mtpole));

   eft->set_g1(sqrt(4.0 * Pi * alpha_sm(0)));
   eft->set_g2(sqrt(4.0 * Pi * alpha_sm(1)));
   eft->set_g3(sqrt(4.0 * Pi * alpha_sm(2)));
   eft->set_scale(mtpole);

   eft->set_v(Electroweak_constants::vev);
   eft->set_Yu(ZEROMATRIX(3,3));
   eft->set_Yd(ZEROMATRIX(3,3));
   eft->set_Ye(ZEROMATRIX(3,3));

   eft->set_Yu(0, 0, -Sqrt(2.)* mu_guess/ eft->get_v());
   eft->set_Yu(1, 1, -Sqrt(2.)* mc_guess/ eft->get_v());
   eft->set_Yu(2, 2, -Sqrt(2.)* mt_guess/ eft->get_v());

   eft->set_Yd(0, 0, Sqrt(2.)* md_guess/ eft->get_v());
   eft->set_Yd(1, 1, Sqrt(2.)* ms_guess/ eft->get_v());
   eft->set_Yd(2, 2, Sqrt(2.)* mb_guess/ eft->get_v());

   eft->set_Ye(0, 0, Sqrt(2.)* me_guess/ eft->get_v());
   eft->set_Ye(1, 1, Sqrt(2.)* mm_guess/ eft->get_v());
   eft->set_Ye(2, 2, Sqrt(2.)* mtau_guess/ eft->get_v());

   eft->set_Lambdax(0.12604);
   eft->solve_ewsb_tree_level();

}

void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_DRbar_yukawa_couplings()
{
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
}

/**
 * Calculates the Yukawa couplings Yu of the up-type quarks
 * from the Standard Model up-type quark masses (ignoring threshold
 * corrections).
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_Yu_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> upQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   upQuarksDRbar(0,0) = mu_guess;
   upQuarksDRbar(1,1) = mc_guess;
   upQuarksDRbar(2,2) = mt_guess;

}

/**
 * Calculates the Yukawa couplings Yd of the down-type
 * quarks from the Standard Model down-type quark masses (ignoring
 * threshold corrections).
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_Yd_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   downQuarksDRbar(0,0) = md_guess;
   downQuarksDRbar(1,1) = ms_guess;
   downQuarksDRbar(2,2) = mb_guess;

}

/**
 * Calculates the Yukawa couplings Ye of the leptons
 * from the Standard Model down-type lepton masses (ignoring threshold
 * corrections).
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_Ye_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downLeptonsDRbar(ZEROMATRIXCOMPLEX(3,3));
   downLeptonsDRbar(0,0) = me_guess;
   downLeptonsDRbar(1,1) = mm_guess;
   downLeptonsDRbar(2,2) = mtau_guess;

}

/**
 * Guesses the full model parameters.  At first it runs to the
 * guess of the SUSY-scale (SUSYScaleFirstGuess) and imposes the
 * SUSY scale initial guess and the
 * SUSY-scale constraint (SUSYScaleInput).  Afterwards, it solves the
 * EWSB conditions at the tree-level.
 * Finally the DR-bar mass spectrum is calculated.
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::guess_model_parameters()
{
   const double susy_scale_guess = susy_constraint.get_initial_scale_guess();
   const auto scale_getter = [this] () { return susy_constraint.get_scale(); };

   model->set_scale(susy_scale_guess);

   // apply susy-scale first guess
   const auto TanBeta = INPUTPARAMETER(TanBeta);
   const auto M1Input = INPUTPARAMETER(M1Input);
   const auto M2Input = INPUTPARAMETER(M2Input);
   const auto M3Input = INPUTPARAMETER(M3Input);
   const auto mq2Input = INPUTPARAMETER(mq2Input);
   const auto mu2Input = INPUTPARAMETER(mu2Input);
   const auto md2Input = INPUTPARAMETER(md2Input);
   const auto ml2Input = INPUTPARAMETER(ml2Input);
   const auto me2Input = INPUTPARAMETER(me2Input);
   const auto MuInput = INPUTPARAMETER(MuInput);
   const auto mAInput = INPUTPARAMETER(mAInput);
   const auto AuInput = INPUTPARAMETER(AuInput);
   const auto AdInput = INPUTPARAMETER(AdInput);
   const auto AeInput = INPUTPARAMETER(AeInput);
   const auto Yu = MODELPARAMETER(Yu);
   const auto Yd = MODELPARAMETER(Yd);
   const auto Ye = MODELPARAMETER(Ye);

   MODEL->set_vu(Re((TanBeta*LowEnergyConstant(vev))/Sqrt(1 + Sqr(TanBeta))));
   MODEL->set_vd(Re(LowEnergyConstant(vev)/Sqrt(1 + Sqr(TanBeta))));
   MODEL->set_MassB(Re(M1Input));
   MODEL->set_MassWB(Re(M2Input));
   MODEL->set_MassG(Re(M3Input));
   MODEL->set_mq2((mq2Input).real());
   MODEL->set_mu2((mu2Input).real());
   MODEL->set_md2((md2Input).real());
   MODEL->set_ml2((ml2Input).real());
   MODEL->set_me2((me2Input).real());
   MODEL->set_Mu(Re(MuInput));
   MODEL->set_BMu(Re(Sqr(mAInput)/(1/TanBeta + TanBeta)));
   MODEL->set_TYu(((AuInput).cwiseProduct(Yu)).real());
   MODEL->set_TYd(((AdInput).cwiseProduct(Yd)).real());
   MODEL->set_TYe(((AeInput).cwiseProduct(Ye)).real());

   eft->run_to(susy_scale_guess, running_precision);
   eft->calculate_DRbar_masses();

   //get gauge and Yukawa couplings from effective theory
   MSSMatMSUSYEFTHiggs_mAmu_standard_model_matching_up<Two_scale> matching_up;
   matching_up.set_models(eft, model);
   matching_up.set_scale(scale_getter);
   matching_up.match_tree_level();

   model->run_to(susy_scale_guess, running_precision);

   // apply susy-scale constraint
   susy_constraint.set_model(model);
   susy_constraint.apply();

   // apply EWSB constraint
   model->solve_ewsb_tree_level();

   // calculate tree-level spectrum
   model->calculate_DRbar_masses();
}

} // namespace flexiblesusy


// Copyright 2011 Google Inc. All Rights Reserved.
//
// 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.
//
// Author: nadavs@google.com <Nadav Samet>
//         Jin Qing (http://blog.csdn.net/jq0123)

#ifndef RPCZ_EVENT_ID_GENERATOR_H
#define RPCZ_EVENT_ID_GENERATOR_H

#include <boost/noncopyable.hpp>
#include "rpcz/common.hpp"

namespace rpcz {

typedef uint64 event_id;

namespace detail {

const uint64 kLargePrime = (1ULL << 63) - 165;
const uint64 kGenerator = 2;

class event_id_generator : boost::noncopyable {
 public:
  event_id_generator();

  event_id get_next() {
    state_ = (state_ * kGenerator) % kLargePrime;
    return state_;
  }

 private:
  uint64 state_;
};  // class event_id_generator

}  // namespace detail
}  // namespace rpcz

#endif  // RPCZ_EVENT_ID_GENERATOR_H


/*
 * Copyright (c) 2018-2019 Tuukka Norri
 * This code is licensed under MIT license (see LICENSE for details).
 */

#include <libbio/algorithm.hh>

#include <boost/dynamic_bitset.hpp>
#include <boost/locale/utf.hpp>
#include <boost/range.hpp>
#include <iostream>
#include <libbio/int_vector.hh>
#include <libbio/map_on_stack.hh>
#include <libbio/mmap_handle.hh>
#include <range/v3/all.hpp>
#include <string>
#include <text_align/code_point_range.hh>
#include <text_align/smith_waterman/aligner.hh>
#include <vector>

#include "cmdline.h"

namespace ta = text_align;

typedef std::int32_t score_type;
enum { SCORE_MAX = std::numeric_limits <score_type>::max() };

template <typename t_word, typename t_delegate>
using aligner_type = ta::smith_waterman::aligner <score_type, t_word, t_delegate>;

typedef ta::smith_waterman::detail::score_result <score_type> score_result;

struct score_container
{
	libbio::matrix <score_type>		score;
	libbio::matrix <score_type>		gap_score_lhs;
	libbio::matrix <score_type>		gap_score_rhs;
	libbio::matrix <std::uint8_t>	max_idx;
	libbio::matrix <std::uint8_t>	did_start_gap;

	score_container() = default;

	void resize(std::size_t const rows, std::size_t const columns)
	{
		score.resize(rows, columns);
		gap_score_lhs.resize(rows, columns);
		gap_score_rhs.resize(rows, columns);
		max_idx.resize(rows, columns);
		did_start_gap.resize(rows, columns);

		std::fill(score.begin(), score.end(), SCORE_MAX);
		std::fill(gap_score_lhs.begin(), gap_score_lhs.end(), SCORE_MAX);
		std::fill(gap_score_rhs.begin(), gap_score_rhs.end(), SCORE_MAX);
		std::fill(max_idx.begin(), max_idx.end(), UINT8_MAX);
		std::fill(did_start_gap.begin(), did_start_gap.end(), UINT8_MAX);
	}
};

// Call boost::asio::io_context::stop when needed.
class aligner_delegate
{
protected:
	boost::asio::io_context	*m_pool{};
	libbio::bit_vector m_lhs_gaps;
	libbio::bit_vector m_rhs_gaps;

public:
	aligner_delegate() = default;
	aligner_delegate(boost::asio::io_context &pool):
		m_pool(&pool)
	{
	}

	libbio::bit_vector const &lhs_gaps() const { return m_lhs_gaps; }
	libbio::bit_vector const &rhs_gaps() const { return m_rhs_gaps; }

	virtual void will_run_aligner(ta::smith_waterman::aligner_base &aligner, std::size_t const lhs_size, std::size_t const rhs_size) {}
	void finish(ta::smith_waterman::aligner_base &aligner) { /* m_pool->stop(); */ }

	void push_lhs(bool flag, std::size_t count) { m_lhs_gaps.push_back(flag, count); }
	void push_rhs(bool flag, std::size_t count) { m_rhs_gaps.push_back(flag, count); }
	void clear_gaps() { m_lhs_gaps.clear(); m_rhs_gaps.clear(); }
	void reverse_gaps() { m_lhs_gaps.reverse(); m_rhs_gaps.reverse(); }

	static constexpr bool uses_scoring_function() { return false; }
};

// Store the alignment scores for later comparison.
class verifying_aligner_delegate final : public aligner_delegate
{
protected:
	score_container	m_scores;

public:
	using aligner_delegate::aligner_delegate;

	score_container const &scores() const { return m_scores; }

	virtual void will_run_aligner(ta::smith_waterman::aligner_base &aligner, std::size_t const lhs_size, std::size_t const rhs_size) override
	{
		// Reset the block size s.t. just one block will be filled.
		aligner.set_segment_length(1 + std::max(lhs_size, rhs_size));

		// Reserve memory for the scores.
		m_scores.resize(1 + lhs_size, 1 + rhs_size);
	}

	void did_calculate_score(ta::smith_waterman::aligner_base &aligner, std::size_t j, std::size_t i, score_result const &result, bool initial)
	{
		if (initial)
		{
			// Store the calculated values.
			libbio_assert(SCORE_MAX == m_scores.score(j, i));
			libbio_assert(SCORE_MAX == m_scores.gap_score_lhs(j, i));
			libbio_assert(SCORE_MAX == m_scores.gap_score_rhs(j, i));
			libbio_assert(UINT8_MAX == m_scores.max_idx(j, i));
			libbio_assert(UINT8_MAX == m_scores.did_start_gap(j, i));

			m_scores.score(j, i) = result.score;
			m_scores.gap_score_lhs(j, i) = result.gap_score_lhs;
			m_scores.gap_score_rhs(j, i) = result.gap_score_rhs;
			m_scores.max_idx(j, i) = result.max_idx;
			m_scores.did_start_gap(j, i) = result.did_start_gap;
		}
		else
		{
			libbio_assert(m_scores.score(j, i) == result.score);
			libbio_assert(m_scores.gap_score_lhs(j, i) == result.gap_score_lhs);
			libbio_assert(m_scores.gap_score_rhs(j, i) == result.gap_score_rhs);
			libbio_assert(m_scores.max_idx(j, i) == result.max_idx);
			libbio_assert(m_scores.did_start_gap(j, i) == result.did_start_gap);
		}
	}
};

// Check that the alignment score matches the previous value.
class tested_aligner_delegate final : public aligner_delegate
{
protected:
	score_container const *m_scores{};

public:
	using aligner_delegate::aligner_delegate;

	void set_scores(score_container const &container) { m_scores = &container; }

protected:
	template <typename t_type>
	void compare(char const *name, std::size_t const j, std::size_t const i, t_type const expected, t_type const actual)
	{
		if (actual != expected)
			std::cerr << name << " mismatch at (" << j << ", " << i << "): expected " << +expected << ", actual " << +actual << ".\n";
	}

public:
	void did_calculate_score(ta::smith_waterman::aligner_base &aligner, std::size_t j, std::size_t i, score_result const &result, bool initial)
	{
		// Compare to the pre-calculated.
		compare("score:        ", j, i, m_scores->score(j, i), result.score);
		compare("gap_score_lhs:", j, i, m_scores->gap_score_lhs(j, i), result.gap_score_lhs);
		compare("gap_score_rhs:", j, i, m_scores->gap_score_rhs(j, i), result.gap_score_rhs);
		compare("max_idx:      ", j, i, m_scores->max_idx(j, i), result.max_idx);
		compare("did_start_gap:", j, i, m_scores->did_start_gap(j, i), static_cast <std::uint8_t>(result.did_start_gap));
	}
};

template <typename t_range>
std::size_t copy_distance(t_range const &range)
{
	return ranges::distance(range);
}

template <typename t_range>
void print_aligned(t_range const &range, libbio::bit_vector const &gaps)
{
	//libbio_assert(str.size() <= gaps.size());

	std::string buffer;

	auto it(range.begin());
	auto const end(range.end());
	auto ostream_it(std::ostream_iterator <char>(std::cout, ""));
	// std::ostream_iterator's operator++ is a no-op, hence we don't
	// save the output value from utf_traits nor use it in else.
	for (auto const val : gaps)
	{
		if (0 == val)
		{
			libbio_assert(it != end);
			boost::locale::utf::utf_traits <char>::encode(*it, ostream_it);
			++it; // Ranges’ iterator’s operator++ returns void.
		}
		else
		{
			std::cout << '-';
		}
	}
	libbio_assert(it == end);
	std::cout << std::endl;
}

struct string_view_from_input
{
	template <typename t_tuple, typename t_fn>
	void operator()(t_tuple const &tuple, t_fn &&fn)
	{
		auto const *arg(std::get <0>(tuple));
		auto const *path_arg(std::get <1>(tuple));

		if (arg)
		{
			std::string_view view(arg, strlen(arg));
			fn(view);
		}
		else
		{
			libbio::mmap_handle <char> handle;
			handle.open(path_arg);
			std::string_view view(handle.data(), handle.size());
			fn(view);
		}
	}
};

template <typename t_aligner>
void configure_aligner(t_aligner &aligner, gengetopt_args_info const &args_info)
{
	aligner.set_segment_length(args_info.block_size_arg);
	aligner.set_identity_score(args_info.match_score_arg);
	aligner.set_mismatch_penalty(args_info.mismatch_penalty_arg);
	aligner.set_gap_start_penalty(args_info.gap_start_penalty_arg);
	aligner.set_gap_penalty(args_info.gap_penalty_arg);
	aligner.set_prints_debugging_information(args_info.print_debugging_information_flag);
	aligner.set_reverses_texts(true);
}

template <typename t_aligner>
void run_aligner(
	t_aligner &aligner,
	typename t_aligner::delegate_type &delegate,
	boost::asio::io_context &pool,
	std::string_view const &lhssv,
	std::string_view const &rhssv,
	gengetopt_args_info const &args_info
)
{
	pool.restart();
	if (args_info.align_bytes_flag)
	{
		auto const lhs_len(lhssv.size());
		auto const rhs_len(rhssv.size());
		auto const lhsr(ranges::view::reverse(lhssv));
		auto const rhsr(ranges::view::reverse(rhssv));
		delegate.will_run_aligner(aligner, lhs_len, rhs_len);
		aligner.align(lhsr, rhsr, lhs_len, rhs_len);
		pool.run();
		std::cout << "Score: " << aligner.alignment_score() << std::endl;
	}
	else
	{
		auto const lhsr(ta::make_reversed_code_point_range(ranges::view::reverse(lhssv)));
		auto const rhsr(ta::make_reversed_code_point_range(ranges::view::reverse(rhssv)));
		auto const lhs_len(copy_distance(lhsr));
		auto const rhs_len(copy_distance(rhsr));
		libbio_assert(lhsr.begin() != lhsr.end());
		libbio_assert(rhsr.begin() != rhsr.end());

		delegate.will_run_aligner(aligner, lhs_len, rhs_len);
		aligner.align(lhsr, rhsr, lhs_len, rhs_len);
		pool.run();
		std::cout << "Score: " << aligner.alignment_score() << std::endl;
	}
}

template <typename t_delegate>
void print_texts_if_needed(
	t_delegate const &delegate,
	std::string_view const &lhssv,
	std::string_view const &rhssv,
	gengetopt_args_info const &args_info
)
{
	if (!args_info.align_bytes_flag)
	{
		auto const lhsv(ta::make_code_point_range(lhssv));
		auto const rhsv(ta::make_code_point_range(rhssv));
		print_aligned(lhsv, delegate.lhs_gaps());
		print_aligned(rhsv, delegate.rhs_gaps());
	}
}

void process_input(boost::asio::io_context &pool, gengetopt_args_info const &args_info)
{
	auto const lhs_input(std::make_tuple(args_info.lhs_arg, args_info.lhs_file_arg));
	auto const rhs_input(std::make_tuple(args_info.rhs_arg, args_info.rhs_file_arg));
	libbio::map_on_stack_fn <string_view_from_input>(
		[&pool, &args_info](std::string_view const &lhsv, std::string_view const &rhsv) {

			// Check whether the alignment should also be verified.
			if (args_info.verify_alignment_flag)
			{
				// Run first the verifying aligner, then compare the results.
				typedef aligner_type <std::uint64_t, verifying_aligner_delegate> verifying_aligner_type;
				typedef aligner_type <std::uint16_t, tested_aligner_delegate> tested_aligner_type;

				verifying_aligner_delegate ad(pool);
				tested_aligner_delegate td(pool);
				td.set_scores(ad.scores());

				{
					verifying_aligner_type verifying_aligner(pool, ad);
					configure_aligner(verifying_aligner, args_info);
					run_aligner(verifying_aligner, ad, pool, lhsv, rhsv, args_info);
					libbio_assert(ad.lhs_gaps().size() == ad.rhs_gaps().size());
				}

				{
					tested_aligner_type tested_aligner(pool, td);
					configure_aligner(tested_aligner, args_info);
					run_aligner(tested_aligner, td, pool, lhsv, rhsv, args_info);
				}

				{
					// Check the gap vectors.
					bool have_mismatch(false);
					auto const &lhs_expected_gaps(ad.lhs_gaps());
					auto const &rhs_expected_gaps(ad.rhs_gaps());
					auto const &lhs_gaps(td.lhs_gaps());
					auto const &rhs_gaps(td.rhs_gaps());
					libbio_assert(lhs_gaps.size() == rhs_gaps.size());
					if (lhs_expected_gaps.size() != lhs_gaps.size())
						std::cerr << "Traceback lengths do not match. Expected " << lhs_expected_gaps.size() << ", got " << lhs_gaps.size() << ".\n";

					{
						std::size_t i(lhs_gaps.size());
						std::size_t j(lhs_expected_gaps.size());
						while (i && j)
						{
							--i;
							--j;
							if (lhs_gaps[i] != lhs_expected_gaps[j])
							{
								have_mismatch = true;
								std::cerr << "Mismatch in lhs gaps at positions " << j << " (expected), " << i << " (actual).\n";
							}
						}
					}

					{
						std::size_t i(rhs_gaps.size());
						std::size_t j(rhs_expected_gaps.size());
						while (i && j)
						{
							--i;
							--j;
							if (rhs_gaps[i] != rhs_expected_gaps[j])
							{
								have_mismatch = true;
								std::cerr << "Mismatch in rhs gaps at positions " << j << " (expected), " << i << " (actual).\n";
							}
						}
					}

					if (have_mismatch)
					{
						std::cerr << "Lhs gaps:          ";
						for (auto const gg : lhs_gaps)
							std::cerr << gg;
						std::cerr << "\nRhs gaps:          ";
						for (auto const gg : rhs_gaps)
							std::cerr << gg;
						std::cerr << "\nLhs expected gaps: ";
						for (auto const gg : lhs_expected_gaps)
							std::cerr << gg;
						std::cerr << "\nRhs expected gaps: ";
						for (auto const gg : rhs_expected_gaps)
							std::cerr << gg;
						std::cerr << '\n';
					}
				}

				print_texts_if_needed(td, lhsv, rhsv, args_info);
			}
			else
			{
				aligner_delegate ad(pool);
				aligner_type <std::uint64_t, aligner_delegate> aligner(pool, ad);
				configure_aligner(aligner, args_info);
				run_aligner(aligner, ad, pool, lhsv, rhsv, args_info);
				print_texts_if_needed(ad, lhsv, rhsv, args_info);
			}
		},
		lhs_input, rhs_input
	);
}

int main(int argc, char **argv)
{
	//std::string lhs("ACTAAGGCTCTCTACCCCTCTCAGAGA");
	//std::string rhs("ACTAAGGCTCCTAACCCCCTTTTCTCAGA");

	gengetopt_args_info args_info;
	if (0 != cmdline_parser(argc, argv, &args_info))
		exit(EXIT_FAILURE);

	std::ios_base::sync_with_stdio(false);	// Don't use C style IO after calling cmdline_parser.
	std::cin.tie(nullptr);					// We don't require any input from the user.

	if (args_info.print_invocation_flag)
	{
		std::cerr << "Invocation:";
		for (int i(0); i < argc; ++i)
			std::cerr << ' ' << argv[i];
		std::cerr << '\n';
	}

	if (args_info.block_size_arg < 0)
	{
		std::cerr << "Block size needs to be non-negative." << std::endl;
		exit(EXIT_FAILURE);
	}

	if (args_info.single_threaded_flag)
	{
		boost::asio::io_context pool(1);
		process_input(pool, args_info);
	}
	else
	{
		boost::asio::io_context pool;
		process_input(pool, args_info);
	}

	cmdline_parser_free(&args_info);

	return 0;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "PointwiseFunctions/AnalyticSolutions/GeneralRelativity/Tov.hpp"

#include <algorithm>
#include <array>
#include <boost/numeric/odeint.hpp>  // IWYU pragma: keep
#include <cmath>
#include <cstddef>
#include <functional>
#include <pup.h>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "Utilities/Gsl.hpp"

// IWYU pragma: no_forward_declare boost::numeric::odeint::controlled_runge_kutta
// IWYU pragma: no_forward_declare EquationsOfState::EquationOfState
// IWYU pragma: no_forward_declare Tensor
// IWYU pragma: no_include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/generation/make_dense_output.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>

/// \cond
namespace {

void lindblom_rhs(
    gsl::not_null<std::array<double, 2>*> dvars,
    const std::array<double, 2>& vars, const double log_enthalpy,
    const std::unique_ptr<EquationsOfState::EquationOfState<true, 1>>&
        equation_of_state) noexcept {
  const double& radius_squared = vars[0];
  const double& mass_over_radius = vars[1];
  double& d_radius_squared = (*dvars)[0];
  double& d_mass_over_radius = (*dvars)[1];
  const double specific_enthalpy = std::exp(log_enthalpy);
  const double rest_mass_density =
      get(equation_of_state->rest_mass_density_from_enthalpy(
          Scalar<double>{specific_enthalpy}));
  const double pressure = get(equation_of_state->pressure_from_density(
      Scalar<double>{rest_mass_density}));
  const double energy_density =
      specific_enthalpy * rest_mass_density - pressure;

  // At the center of the star: (u,v) = (0,0)
  if (UNLIKELY((radius_squared == 0.0) and (mass_over_radius == 0.0))) {
    d_radius_squared = -3.0 / (2.0 * M_PI * (energy_density + 3.0 * pressure));
    d_mass_over_radius =
        -2.0 * energy_density / (energy_density + 3.0 * pressure);
  } else {
    const double common_factor =
        (1.0 - 2.0 * mass_over_radius) /
        (4.0 * M_PI * radius_squared * pressure + mass_over_radius);
    d_radius_squared = -2.0 * radius_squared * common_factor;
    d_mass_over_radius =
        -(4.0 * M_PI * radius_squared * energy_density - mass_over_radius) *
        common_factor;
  }
}

class Observer {
 public:
  void operator()(const std::array<double, 2>& vars,
                  const double current_log_enthalpy) noexcept {
    radius.push_back(std::sqrt(vars[0]));
    mass.push_back(std::sqrt(vars[0]) * vars[1]);
    log_enthalpy.push_back(current_log_enthalpy);
  }
  std::vector<double> radius;
  std::vector<double> mass;
  std::vector<double> log_enthalpy;
};

}  // namespace

namespace gr {
namespace Solutions {

TovSolution::TovSolution(
    const std::unique_ptr<EquationsOfState::EquationOfState<true, 1>>&
        equation_of_state,
    const double central_mass_density, const double final_log_enthalpy,
    const double absolute_tolerance, const double relative_tolerance) {
  std::array<double, 2> u_and_v = {{0.0, 0.0}};
  std::array<double, 2> dudh_and_dvdh{};
  const double central_log_enthalpy =
      std::log(get(equation_of_state->specific_enthalpy_from_density(
          Scalar<double>{central_mass_density})));
  lindblom_rhs(&dudh_and_dvdh, u_and_v, central_log_enthalpy,
               equation_of_state);
  const double initial_step = -std::min(std::abs(1.0 / dudh_and_dvdh[0]),
                                        std::abs(1.0 / dudh_and_dvdh[1]));
  using StateDopri5 =
      boost::numeric::odeint::runge_kutta_dopri5<std::array<double, 2>>;
  boost::numeric::odeint::dense_output_runge_kutta<
      boost::numeric::odeint::controlled_runge_kutta<StateDopri5>>
      dopri5 = make_dense_output(absolute_tolerance, relative_tolerance,
                                 StateDopri5{});
  Observer observer{};
  boost::numeric::odeint::integrate_adaptive(
      dopri5,
      [&equation_of_state](const std::array<double, 2>& lindblom_u_and_v,
                           std::array<double, 2>& lindblom_dudh_and_dvdh,
                           const double lindblom_enthalpy) noexcept {
        return lindblom_rhs(&lindblom_dudh_and_dvdh, lindblom_u_and_v,
                            lindblom_enthalpy, equation_of_state);
      },
      u_and_v, central_log_enthalpy, final_log_enthalpy, initial_step,
      std::ref(observer));
  outer_radius_ = observer.radius.back();
  mass_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.mass, 5);
  // log_enthalpy(radius) is almost linear so an interpolant of order 3
  // maximizes precision
  log_enthalpy_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.log_enthalpy, 3);
}

double TovSolution::outer_radius() const noexcept { return outer_radius_; }

double TovSolution::mass(const double r) const noexcept {
  return mass_interpolant_(r);
}

Scalar<DataVector> TovSolution::mass(const Scalar<DataVector>& radius) const
    noexcept {
  DataVector mass(radius.size(), 0.0);
  for (size_t i = 0; i < radius.size(); i++) {
    mass[i] = mass_interpolant_(get(radius)[i]);
  }
  return Scalar<DataVector>{std::move(mass)};
}

double TovSolution::log_specific_enthalpy(const double r) const noexcept {
  return log_enthalpy_interpolant_(r);
}

Scalar<DataVector> TovSolution::log_specific_enthalpy(
    const Scalar<DataVector>& radius) const noexcept {
  DataVector log_specific_enthalpy(radius.size(), 0.0);
  for (size_t i = 0; i < radius.size(); i++) {
    log_specific_enthalpy[i] = log_enthalpy_interpolant_(get(radius)[i]);
  }
  return Scalar<DataVector>{std::move(log_specific_enthalpy)};
}

double TovSolution::specific_enthalpy(const double r) const noexcept {
  return std::exp(log_enthalpy_interpolant_(r));
}

Scalar<DataVector> TovSolution::specific_enthalpy(
    const Scalar<DataVector>& radius) const noexcept {
  return Scalar<DataVector>{
      exp(get(TovSolution::log_specific_enthalpy(radius)))};
}

void TovSolution::pup(PUP::er& p) noexcept {  // NOLINT
  p | outer_radius_;
  p | mass_interpolant_;
  p | log_enthalpy_interpolant_;
}
}  // namespace Solutions
}  // namespace gr
/// \endcond


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2017-2019 The papara developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"
#include "spork.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    CAmount nSubsidy = 0;

    nSubsidy = GetBlockValue(0);
    BOOST_CHECK(nSubsidy == 0.1 * COIN);
    nSum += nSubsidy;

    /* premine in block 1 (3,000,000 papara) */
    nSubsidy = GetBlockValue(1);
    BOOST_CHECK(nSubsidy == 3000000 * COIN);
    nSum += nSubsidy;

    nSubsidy = GetBlockValue(0);
    BOOST_CHECK(nSubsidy == 0.1 * COIN);
    nSum += nSubsidy;

    for (int nHeight = 3; nHeight <= 291; nHeight += 1) {
        /* PoW Phase */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 3 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 292; nHeight <= 1728; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 2 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1729; nHeight <= 4608; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 5 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 4609; nHeight <= 8928; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 10 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 8929; nHeight <= 11808; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 25 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 11809; nHeight <= 16128; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 70 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 16129; nHeight <= 19008; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 100 * COIN);
        nSum += nSubsidy;
    }
    //Until TIER_BLOCK_HEIGHT
    for (int nHeight = 19009; nHeight <= 524500; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 200 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 524501; nHeight <= 524500 + 100; nHeight += 1) {
        /* Masternode tiers system implemented */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 100 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1578902; nHeight <= 1578902 + 100; nHeight += 1) {
        /* Masternode tiers system implemented */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 25 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1578902; nHeight < SPORK_21_SUPERBLOCK_START_DEFAULT; nHeight += 1) {
        /* Standard reward of 25 coin until 21th spork and halving */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 25 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = SPORK_21_SUPERBLOCK_START_DEFAULT; nHeight < SPORK_20_REWARD_HALVING_START_DEFAULT+SPORK_20_REWARD_HALVING_PERIOD_DEFAULT; nHeight += 1) {
        /* Standard reward of 1000 or 300000 coin after 21th spork and halving */
        CAmount nSubsidy = GetBlockValue(nHeight);
        if (nHeight % SPORK_21_SUPERBLOCK_PERIOD_DEFAULT == 0) {
            BOOST_CHECK(nSubsidy == 300000 * COIN);
        }else{
            BOOST_CHECK(nSubsidy == 1000 * COIN);
        }

        nSum += nSubsidy;
    }
}

BOOST_AUTO_TEST_CASE(halving_test)
{
    int blockBeforeSporkAndHalving = (SPORK_20_REWARD_HALVING_START_DEFAULT + (SPORK_21_SUPERBLOCK_START_DEFAULT - SPORK_20_REWARD_HALVING_START_DEFAULT)) - 10;

    BOOST_CHECK(GetHalvingReward(0, SPORK_20_REWARD_VALUE) == SPORK_20_REWARD_VALUE);
    BOOST_CHECK(GetHalvingReward(SPORK_20_REWARD_HALVING_START_DEFAULT, SPORK_20_REWARD_VALUE) == SPORK_20_REWARD_VALUE);
    /* Before 21 spork block reward is still 25 and default because halving will have not reached */
    BOOST_CHECK(GetHalvingReward(blockBeforeSporkAndHalving, SPORK_20_REWARD_VALUE) == SPORK_20_REWARD_VALUE);
    /* On 21 spork block reward is new but default because halving will have not reached */
    BOOST_CHECK(GetHalvingReward(SPORK_21_SUPERBLOCK_START_DEFAULT, SPORK_21_REWARD_VALUE) == SPORK_21_REWARD_VALUE);
    /* Cases when 21 spork and halving are reached */
    BOOST_CHECK(GetHalvingReward(1578902 + 525600, SPORK_21_REWARD_VALUE) == (SPORK_21_REWARD_VALUE / 2)); //
    BOOST_CHECK(GetHalvingReward(1578902 + 525600 + 525600 + 525600 + 525600, SPORK_21_REWARD_VALUE) == (SPORK_21_REWARD_VALUE / 5));
}

BOOST_AUTO_TEST_CASE(superblock_halving_test)
{
    double standartReward = 300000;

    BOOST_CHECK(GetSuperblockHalvingReward(0) == standartReward);
    BOOST_CHECK(GetSuperblockHalvingReward( (SPORK_21_SUPERBLOCK_START_DEFAULT / SPORK_21_SUPERBLOCK_PERIOD_DEFAULT + 1) * SPORK_21_SUPERBLOCK_PERIOD_DEFAULT) == standartReward);
    BOOST_CHECK(GetSuperblockHalvingReward(((SPORK_21_SUPERBLOCK_START_DEFAULT + 525600) / SPORK_21_SUPERBLOCK_PERIOD_DEFAULT + 1) * SPORK_21_SUPERBLOCK_PERIOD_DEFAULT) == (standartReward / 2));
    BOOST_CHECK(GetSuperblockHalvingReward(((SPORK_21_SUPERBLOCK_START_DEFAULT + 525600 * 4) / SPORK_21_SUPERBLOCK_PERIOD_DEFAULT + 1) * SPORK_21_SUPERBLOCK_PERIOD_DEFAULT) == (standartReward / 5));
}

BOOST_AUTO_TEST_CASE(charity_address_valid_test)
{
    CScript payee;
    BOOST_CHECK(GetCharityPayee(SPORK_21_SUPERBLOCK_START_DEFAULT, payee) == true);
}

BOOST_AUTO_TEST_CASE(charity_address_invalid_test)
{
    CScript payee;
    BOOST_CHECK(GetCharityPayee(SPORK_20_REWARD_HALVING_START_DEFAULT, payee) == false);
}

BOOST_AUTO_TEST_SUITE_END()


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"

#include <boost/python.hpp>

#include "graph.hh"
#include "graph_selectors.hh"
#include "graph_hits.hh"

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_hits_dispatch
{
    template <class Graph, class VertexIndex, class WeightMap,
              class CentralityMap>
    void operator()(Graph& g, VertexIndex vertex_index, WeightMap w,
                    CentralityMap x, boost::any ay, double epsilon,
                    size_t max_iter, long double& eig) const
    {
        try
        {
            typename CentralityMap::checked_t y = any_cast<typename CentralityMap::checked_t>(ay);
            get_hits()(g, vertex_index, w, x,
                       y.get_unchecked(num_vertices(g)), epsilon, max_iter,
                       eig);
        }
        catch (bad_any_cast&)
        {
            throw GraphException("x and y vertex properties must be of the same type.");
        }
    }
};

long double hits(GraphInterface& g, boost::any w, boost::any x, boost::any y,
                 double epsilon, size_t max_iter)
{
    if (!w.empty() && !belongs<writable_edge_scalar_properties>()(w))
        throw ValueException("edge property must be writable");
    if (!belongs<vertex_floating_properties>()(x))
        throw ValueException("vertex property must be of floating point"
                             " value type");

    typedef ConstantPropertyMap<int, GraphInterface::edge_t> weight_map_t;
    typedef boost::mpl::push_back<writable_edge_scalar_properties, weight_map_t>::type
        weight_props_t;

    if(w.empty())
        w = weight_map_t(1);

    long double eig = 0;
    run_action<>()
        (g, std::bind(get_hits_dispatch(), placeholders::_1, g.GetVertexIndex(),
                      placeholders::_2,  placeholders::_3, y, epsilon, max_iter,
                      std::ref(eig)),
         weight_props_t(),
         vertex_floating_properties())(w, x);
    return eig;
}

void export_hits()
{
    using namespace boost::python;
    def("get_hits", &hits);
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SHR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SHR_HPP_INCLUDED

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

  /*!

    @ingroup group-operator
    This function object returns the first parameter *logically* shifted right
    by value of the second one.

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

    @par Notes:
      - On many architectures a scalar second parameter results in a much faster call.
      - If the second entry is negative the result is not defined.

      @pre the second parameter must be of integer type, scalar or associated to
      the first parameter

    @par Note:
      - on many architectures a scalar second parameter results in a much faster call

    @see  shift_right, shift_left, rshl, rshr, rol, ror

    @par Example:

      @snippet shr.cpp shr

    @par Possible output:

      @snippet shr.txt shr

  **/
  Value shr(Value const& x, IntegerValue const& n);
} }
#endif

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

#endif


#include "mantella_bits/optimisationAlgorithm/randomSearch.hpp"

// C++ standard library
#include <functional>
#include <string>
#include <utility>

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationProblem.hpp"
#include "mantella_bits/probability.hpp"

namespace mant {
  RandomSearch::RandomSearch()
      : OptimisationAlgorithm() {
    setNextParametersFunctions(
        {{[this](
              const arma::uword numberOfDimensions_,
              const arma::mat& parameters_,
              const arma::rowvec& objectiveValues_,
              const arma::rowvec& differences_) {
            return uniformRandomNumbers(
                numberOfDimensions_);
          },
          "Random search"}});
  }

  void RandomSearch::optimise(
      OptimisationProblem& optimisationProblem) {
    optimise(optimisationProblem, uniformRandomNumbers(optimisationProblem.numberOfDimensions_));
  }
}


// Copyright (c) 2013 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/test/unit_test.hpp>
#include "../crp/CoinReleasePlan.h"

using namespace std;
using namespace crp;

BOOST_AUTO_TEST_SUITE(crp_tests)

BOOST_AUTO_TEST_CASE(test_HeightRange)
{
	HeightRange r;
	BOOST_CHECK(r.Begin()==0);
	BOOST_CHECK(r.Back()==HeightRange::InfiniteHeight());
	BOOST_CHECK(r.End()==HeightRange::InfiniteHeight());
	BOOST_CHECK(r.Exists(-1)==false);
	BOOST_CHECK(r.Exists(0));
	BOOST_CHECK(r.Exists(1));
	BOOST_CHECK(r.Exists(std::numeric_limits<Height>::max()-1));
}

BOOST_AUTO_TEST_CASE(test_HeightRange_range)
{
	HeightRange r;
	r.begin = 1;
	r.range = 2;
	BOOST_CHECK(r.Begin()==1);
	BOOST_CHECK(r.Back()==2);
	BOOST_CHECK(r.End()==3);
	BOOST_CHECK(r.Exists(0)==false);
	BOOST_CHECK(r.Exists(1));
	BOOST_CHECK(r.Exists(2));
	BOOST_CHECK(r.Exists(3)==false);
}

BOOST_AUTO_TEST_CASE(test_DistributeRule) {
	std::vector<Agent> blockCountVec;
	for(int i=0; i<200; i++){
		Agent a;
		a.id = i;
		a.weight = 1;
		a.blockAmount = 1;
		a.lastBlockTime = 1;
		blockCountVec.push_back(a);
	}
	CoinAmount topFirstTotalAmount = 10000;
	CoinAmount topSecondTotalAmount = 10000;

	std::vector<std::pair<Agentid,CoinAmount>> result;

	result = GetMonthlyDistribution(blockCountVec,topFirstTotalAmount,topSecondTotalAmount,DistributeRule(27,72),0);
	BOOST_CHECK(result.size() == 99);

	result = GetMonthlyDistribution(blockCountVec,topFirstTotalAmount,topSecondTotalAmount,DistributeRule(50,100),0);
	BOOST_CHECK(result.size() == 150);
}

BOOST_AUTO_TEST_CASE(test_GetMonthlyDistribution) {
	std::vector<Agent> blockCountVec;
	for(int i=0; i<5; i++){
		Agent a;
		a.id = i;
		a.weight = 1;
		a.blockAmount = 1;
		a.lastBlockTime = 1;
		blockCountVec.push_back(a);
	}
	CoinAmount topFirstTotalAmount = 10000;
	CoinAmount topSecondTotalAmount = 10000;

	std::vector<std::pair<Agentid,CoinAmount>> result;
	result = GetMonthlyDistribution(blockCountVec,topFirstTotalAmount,topSecondTotalAmount,DistributeRule(2,2),0);
	BOOST_CHECK(result.size() == 4);
	BOOST_CHECK(result[0].second == 5000);
	BOOST_CHECK(result[1].second == 5000);
	BOOST_CHECK(result[2].second == 5000);
	BOOST_CHECK(result[3].second == 5000);

	result = GetMonthlyDistribution(blockCountVec,topFirstTotalAmount,topSecondTotalAmount,DistributeRule(2,4),0);
	BOOST_CHECK(result.size() == 5);
	BOOST_CHECK(result[0].second == 5000);
	BOOST_CHECK(result[1].second == 5000);
	BOOST_CHECK(result[2].second == 3334);
	BOOST_CHECK(result[3].second == 3333);
	BOOST_CHECK(result[4].second == 3333);
}

BOOST_AUTO_TEST_CASE(test_DistributeRule_TotalCount) {
	int n1 = 50, n2 = 100;
	DistributeRule rule(n1,n2);
	BOOST_CHECK(rule.TotalCount() == (n1+n2));
}

BOOST_AUTO_TEST_CASE(PosPlanV3) {
	Height forkheight = 676481;

	BlockValue bv;

	bv = GetBlockValue(forkheight-1);
 	BOOST_CHECK_EQUAL(bv.miner,         1.20 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.fund,          0.35 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.entrust,       6.60 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.masternode,    0.48 *COIN / 2);
//	BOOST_CHECK_EQUAL(bv.topFirst,  72800.00 *COIN / 2);
//	BOOST_CHECK_EQUAL(bv.topSecond, 72800.00 *COIN / 2);

	bv = GetBlockValue(forkheight);
 	BOOST_CHECK_EQUAL(bv.miner,         0.48 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.entrust,       5.35 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.fund,          4.16 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.masternode,    0.24 *COIN / 2);
//	BOOST_CHECK_EQUAL(bv.topFirst,  37160.00 *COIN / 2);
//	BOOST_CHECK_EQUAL(bv.topSecond, 37160.00 *COIN / 2);

	bv = GetBlockValue(forkheight+1);
 	BOOST_CHECK_EQUAL(bv.miner,         0.48 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.entrust,       5.35 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.fund,          4.16 *COIN / 2);
 	BOOST_CHECK_EQUAL(bv.masternode,    0.24 *COIN / 2);
//	BOOST_CHECK_EQUAL(bv.topFirst,  37160.00 *COIN / 2);
//	BOOST_CHECK_EQUAL(bv.topSecond, 37160.00 *COIN / 2);
}

BOOST_AUTO_TEST_SUITE_END()