/// Returns the original version of the decision variables ready to be fed
/// to the original problem
decision_vector rotated::derotate(const decision_vector& x_normed) const
{
	// This may be outside of the original domain, due to the 
	// relaxed variable bounds after rotation -- project it back if so.

	// 1. De-rotate the vector in the normalized space
	Eigen::VectorXd x_normed_vec = Eigen::VectorXd::Zero(x_normed.size());
	Eigen::VectorXd x_derotated_vec;
	for(base::size_type i = 0; i < x_normed.size(); i++){
		x_normed_vec(i) = x_normed[i];	
	}
	x_derotated_vec = m_InvRotate * x_normed_vec;

	// 2. De-normalize the de-rotated vector to the original bounds
	decision_vector x_derotated(x_normed.size(), 0);
	for(base::size_type i = 0; i < x_normed.size(); i++){
		x_derotated[i] = x_derotated_vec(i);
	}
	decision_vector x_wild = denormalize_to_original(x_derotated);

	// 3. The de-normalized vector may be out of bounds, if so project back in
	decision_vector x = projection_via_clipping(x_wild);

	return x;
}

/// Apply noise on the decision vector based on rho
void robust::inject_noise_x(decision_vector &x) const
{
	// We follow the algorithm at
	// http://math.stackexchange.com/questions/87230/picking-random-points-in-the-volume-of-sphere-with-uniform-probability

	// 0. Define the radius
	double radius = m_rho * pow(m_uniform_dist(m_drng),1.0/x.size());

	// 1. Sampling N(0,1) on each dimension
	std::vector<double> perturbation(x.size(), 0.0);
	double c2=0;
	for(size_type i = 0; i < perturbation.size(); i++){
		perturbation[i] = m_normal_dist(m_drng);
		c2 += perturbation[i]*perturbation[i];
	}

	// 2. Normalize the vector
	for(size_type i = 0; i < perturbation.size(); i++){
		perturbation[i] *= (radius / sqrt(c2) );
		x[i] += perturbation[i];
	}

	// 3. Clip the variables to the valid bounds
	for(base::size_type i = 0; i < x.size(); i++){
		x[i] = std::max(x[i], get_lb()[i]);
		x[i] = std::min(x[i], get_ub()[i]);
	}
}

// Used to normalize the original upper and lower bounds
// to [-1, 1], at each dimension
decision_vector rotated::normalize_to_center(const decision_vector& x) const
{
	decision_vector normalized_x(x.size(), 0);
	for(base::size_type i = 0; i < x.size(); i++) {
		if (m_normalize_scale[i] == 0) { //If the bounds witdth is zero
			normalized_x[i] = 0;
		} 
		else {
			normalized_x[i] = (x[i] - m_normalize_translation[i]) / m_normalize_scale[i];
		}
	}
	return normalized_x;
}

double antibodies_problem::compute_distance(const decision_vector &x) const {
	double distance = 0.;

	// hamming distance

	switch(m_method) {
	case(algorithm::cstrs_immune_system::HAMMING): {
		const decision_vector &lb = get_lb();
		const decision_vector &ub = get_ub();

		for(decision_vector::size_type i=0; i<x.size(); i++) {

			std::vector<int> current_binary_gene = double_to_binary(x.at(i), lb.at(i), ub.at(i));

			for(decision_vector::size_type j=0; j<m_pop_antigens.size(); j++) {

				std::vector<int> antigens_binary_gene = double_to_binary((m_pop_antigens.at(j)).at(i), lb.at(i), ub.at(i));

				for(std::vector<int>::size_type k=0; k<antigens_binary_gene.size(); k++) {
					distance += antigens_binary_gene.at(k) && current_binary_gene.at(k);
				}
			}
		}

		// we take the inverse of the distance as the measure we need is
		// how close the x is from the antigen population
		// which means that we need to maximize the ressemblance
		distance = - distance;
		break;
	}
	case(algorithm::cstrs_immune_system::EUCLIDEAN): {
		for(decision_vector::size_type j=0; j<m_pop_antigens.size(); j++) {

			double euclid = 0.;
			const decision_vector &antigen_decision = m_pop_antigens.at(j);

			for(decision_vector::size_type i=0; i<x.size(); i++) {
				euclid += std::pow(x.at(i) - antigen_decision.at(i),2);
			}
			distance += std::sqrt(euclid);
		}

		break;
	}
	}

	return distance;
}

// Write into retval the gradient of the continuous part of the objective function of prob calculated in input.
void gsl_gradient::objfun_numdiff_central(gsl_vector *retval, const problem::base &prob, const decision_vector &input, const double &step_size)
{
	if (input.size() != prob.get_dimension()) {
		pagmo_throw(value_error,"invalid input vector dimension in numerical differentiation of the objective function");
	}
	if (prob.get_f_dimension() != 1) {
		pagmo_throw(value_error,"numerical differentiation of the objective function cannot work on multi-objective problems");
	}
	// Size of the continuous part of the problem.
	const problem::base::size_type cont_size = prob.get_dimension() - prob.get_i_dimension();
	// Structure to pass data to the wrapper.
	objfun_numdiff_wrapper_params pars;
	pars.x = input;
	pars.f.resize(1);
	pars.prob = &prob;
	// GSL function.
	gsl_function F;
	F.function = &objfun_numdiff_wrapper;
	F.params = (void *)&pars;
	double result, abserr;
	// Numerical differentiation component by component.
	for (problem::base::size_type i = 0; i < cont_size; ++i) {
		pars.coord = i;
		gsl_deriv_central(&F,input[i],step_size,&result,&abserr);
		gsl_vector_set(retval,i,result);
	}
}

/**
 * @return the encoded string (std::string)
 */
std::string string_match::pretty(const decision_vector &x) const {
	std::string retval;
	for (decision_vector::size_type i = 0; i < x.size(); ++i) {
		retval += char(x[i]);
	}
	return retval;
}

/// Implementation of the objective function.
void lavor_maculan::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	const decision_vector::size_type n = x.size();
	f[0] = 0.0;
	for (decision_vector::size_type i = 0 ; i < n ; ++i) {
		f[0] += 1 + cos(3 * x[i]) + (((i % 2 == 1) ? 1 : -1) / (sqrt(10.60099896 - 4.141720682 * cos(x[i]))));
	}
}

/// Implementation of the objective function.
void rosenbrock::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	const decision_vector::size_type n = x.size();
	f[0]=0;
	for (decision_vector::size_type i=0; i<n-1; ++i){
		f[0] += 100 * (x[i]*x[i] -x[i+1])*(x[i]*x[i] -x[i+1]) + (x[i]-1)*(x[i]-1);
	}
}

// For a decision_vector in the normalized [-1, 1] space,
// perform the inverse operation of normalization to get it's
// location in the original space
decision_vector rotated::denormalize_to_original(const decision_vector& x_normed) const
{
	decision_vector denormalized_x(x_normed.size(), 0);
	for(base::size_type i = 0; i < x_normed.size(); i++){
		denormalized_x[i] = (x_normed[i] * m_normalize_scale[i]) + m_normalize_translation[i];
	}
	return denormalized_x;
}

/// Implementation of the constraint function.
void luksan_vlcek_3::compute_constraints_impl(constraint_vector &c, const decision_vector &x) const
{
	int n = x.size();
	c[0] = 3.*std::pow(x[0],3) + 2.*x[1] - 5. + std::sin(x[0]-x[1])*std::sin(x[0]+x[1]) - m_cub[0];
	c[1] = m_clb[0] - ( 3.*std::pow(x[0],3) + 2.*x[1] - 5. + std::sin(x[0]-x[1])*std::sin(x[0]+x[1]) );
	c[2] = 4.*x[n-3] - x[n-4]*std::exp(x[n-4]-x[n-3]) - 3 - m_cub[1];
	c[3] = m_clb[1] - ( 4.*x[n-3] - x[n-4]*std::exp(x[n-4]-x[n-3]) - 3 );
}

/// Implementation of the objective function.
void sample_return::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	//We split the decision vector in the two legs
	std::copy(x.begin(),x.begin()+6,x_leg1.begin());
	std::copy(x.begin()+6,x.begin()+12,x_leg2.begin());

	x_leg1[4] = x_leg1[4] * m_Tmax;
	x_leg2[4] = (m_Tmax - x_leg1[4] - x_leg2[0]) * x_leg2[4];

	//We account for the waiting time
	x_leg2[0] += x_leg1[0] + x_leg1[4];
	double dummy = 0;
	MGA_DSM(x_leg1, m_leg1, dummy);
	MGA_DSM(x_leg2, m_leg2, dummy);
	f[0] = m_leg1.DV[0] + m_leg1.DV[1] + m_leg1.DV[2] +
		m_leg2.DV[0] + m_leg2.DV[1] + std::max(0.0,m_leg2.DV[2] - 5.5);

}

/// Implementation of the constraint function.
void earth_planet::compute_constraints_impl(constraint_vector &c, const decision_vector &x) const
{
	// We decode the decision vector into a multiple fly-by trajectory
	trajectory.init_from_full_vector(x.begin(),x.end(),encoding);

	// We evaluate the state mismatch at the mid-point. And we use astronomical units to scale them
	trajectory.evaluate_all_mismatch_con(c.begin(), c.begin() + 7);
	for (int i=0; i<3; ++i) c[i]/=ASTRO_AU;
	for (int i=3; i<6; ++i) c[i]/=ASTRO_EARTH_VELOCITY;

	// We evaluate the constraints on the throttles writing on the 7th mismatch constrant (mass is off)
	trajectory.get_leg(0).get_throttles_con(c.begin() + 6, c.begin() + 6 + n_segments);

	// We evaluate the constraint on the initial launch velocity
	c[6 + n_segments] = (trajectory.evaluate_leg_vinf2_i(0) - vmax*vmax) / ASTRO_EARTH_VELOCITY / ASTRO_EARTH_VELOCITY;

	// We evaluate the linear constraint on the epochs (tf > ti)
	c[7 + n_segments] = trajectory.get_leg(0).get_t_i().mjd2000() - trajectory.get_leg(0).get_t_f().mjd2000();
}

/// Implementation of the objective function.
void rastrigin::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	pagmo_assert(f.size() == 1);
	const double omega = 2.0 * boost::math::constants::pi<double>();
	f[0] = 0;
	const decision_vector::size_type n = x.size();
	for (decision_vector::size_type i = 0; i < n; ++i) {
		f[0] += x[i] * x[i] - 10.0 * std::cos(omega * x[i]);
	}
	f[0] += 10.0 * n;
}

/// Implementation of the objective function.
void michalewicz::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	pagmo_assert(f.size() == 1);
	decision_vector::size_type n = x.size();
	double retval = 0.0;

	for (decision_vector::size_type i=0; i<n; i++){
		retval -= sin(x[i]) * pow(sin((i+1)*x[i]*x[i]/boost::math::constants::pi<double>()) , 2*m_m);
	}
	f[0] = retval;
}

/// Implementation of the objective function.
void schwefel::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	pagmo_assert(f.size() == 1);
	std::vector<double>::size_type n = x.size();
	double value=0;

	for (std::vector<double>::size_type i=0; i<n; i++){
		value += x[i] * sin(sqrt(fabs(x[i])));
		}
		f[0] = 418.9828872724338 * n - value;
}

/// Implementation of the objective function.
void dejong::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	pagmo_assert(f.size() == 1);
	decision_vector::size_type n = x.size();
	double retval = 0.0;

	for (decision_vector::size_type i=0; i<n; i++){
		retval += x[i]*x[i];
	}
	f[0] = retval;
}

std::string gtoc5_rendezvous::pretty(const decision_vector &x) const
{
	using namespace kep_toolbox;
	// We set the leg.
	const epoch epoch_i(x[0],epoch::MJD), epoch_f(x[1] + x[0],epoch::MJD);
	array3D v0, r0, vf, rf;
	m_source.eph(epoch_i,r0,v0);
	m_target.eph(epoch_f,rf,vf);
	m_leg.set_leg(epoch_i,sc_state(r0,v0,m_leg.get_spacecraft().get_mass()),x.begin() + 3, x.end(),epoch_f,sc_state(rf,vf,x[2]),ASTRO_MU_SUN);

	std::ostringstream oss;
	oss << m_leg << '\n' << m_source << '\n' << m_target << '\n';
	return oss.str();
}

/// Implementation of the objective function.
void zdt2::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	pagmo_assert(f.size() == 2);
	pagmo_assert(x.size() == 30);

	double g = 0;

	f[0] = x[0];

	for(problem::base::size_type i = 2; i < 30; ++i) {
		g += x[i];
	}
	g = 1 + (9 * g) / 29;
	
	f[1] = g * ( 1 - (x[0]/g)*(x[0]/g));
	
}

/// Implementation of the objective function.
void zdt6::objfun_impl(fitness_vector &f, const decision_vector &x) const
{
	pagmo_assert(f.size() == 2);
	pagmo_assert(x.size() == 10);

	double g = 0;

	f[0] = 1 - exp(-4*x[0])*pow(sin(6*m_pi*x[0]),6);

	for(problem::base::size_type i = 2; i < 10; ++i) {
		g += x[i];
	}
	g = 1 + (9 * g) / 9;
	
	f[1] = g * ( 1 - (f[0]/g)*(f[0]/g));
	
}

std::string gtoc5_launch::pretty(const decision_vector &x) const
{
	using namespace kep_toolbox;
	// 1 - We set the leg.
	const epoch epoch_i(x[0],epoch::MJD), epoch_f(x[1] + x[0],epoch::MJD);
	array3D v0, r0, vf, rf;
	m_earth.get_eph(epoch_i,r0,v0);
	m_target.get_eph(epoch_f,rf,vf);

	v0[0] += x[2];
	v0[1] += x[3];
	v0[2] += x[4];
	m_leg.set_leg(epoch_i,sc_state(r0,v0,m_leg.get_spacecraft().get_mass()),x.begin() + 6, x.end(),epoch_f,sc_state(rf,vf,x[5]),ASTRO_MU_SUN);

	std::ostringstream oss;
	oss << m_leg << '\n' << m_earth << '\n' << m_target << '\n';
	return oss.str();
}