#include "MeshCuboidNonLinearSolver.h"

#include <bitset>

#include <Eigen/Eigenvalues>

MeshCuboidNonLinearSolver::MeshCuboidNonLinearSolver(
	const std::vector<MeshCuboid *>& _cuboids,
	const std::vector<MeshCuboidReflectionSymmetryGroup *>& _reflection_symmetry_groups,
	const std::vector<MeshCuboidRotationSymmetryGroup *>& _rotation_symmetry_groups,
	const Real _squared_neighbor_distance,
	const unsigned int _min_num_symmetry_point_pairs,
	const Real _symmetry_energy_term_weight)
	: cuboids_(_cuboids)
	, reflection_symmetry_groups_(_reflection_symmetry_groups)
	, rotation_symmetry_groups_(_rotation_symmetry_groups)
	, squared_neighbor_distance_(_squared_neighbor_distance)
	, min_num_symmetric_point_pairs_(_min_num_symmetry_point_pairs)
	, symmetry_energy_term_weight_(_symmetry_energy_term_weight)
	, num_cuboid_corner_variables_(MeshCuboidAttributes::k_num_attributes)
	, num_cuboid_axis_variables_(3 * 3)
	, num_reflection_symmetry_group_variables_(MeshCuboidReflectionSymmetryGroup::num_axis_parameters())
	, num_rotation_symmetry_group_variables_(MeshCuboidRotationSymmetryGroup::num_axis_parameters())
{
	num_cuboids_ = _cuboids.size();
	//assert(num_cuboids_ > 0);

	num_reflection_symmetry_groups_ = _reflection_symmetry_groups.size();
	//assert(num_reflection_symmetry_groups_ > 0);
	num_rotation_symmetry_groups_ = _rotation_symmetry_groups.size();
	//assert(num_rotation_symmetry_groups_ > 0);

	cuboid_corner_variable_start_index_ = 0;
	cuboid_axis_variable_start_index_ = cuboid_corner_variable_start_index_
		+ num_total_cuboid_corner_variables();
	reflection_symmetry_group_variable_start_index_ = cuboid_axis_variable_start_index_
		+ num_total_cuboid_axis_variables();
	rotation_symmetry_group_variable_start_index_ = reflection_symmetry_group_variable_start_index_
		+ num_total_reflection_symmetry_group_variables();
}

MeshCuboidNonLinearSolver::~MeshCuboidNonLinearSolver()
{

}

unsigned int MeshCuboidNonLinearSolver::num_total_cuboid_corner_variables() const
{
	return num_cuboid_corner_variables_ * num_cuboids_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_cuboid_axis_variables() const
{
	return num_cuboid_axis_variables_ * num_cuboids_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_reflection_symmetry_group_variables() const
{
	return num_reflection_symmetry_groups_ * num_reflection_symmetry_group_variables_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_rotation_symmetry_group_variables() const
{
	return num_rotation_symmetry_groups_ * num_rotation_symmetry_group_variables_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_variables() const
{
	return num_total_cuboid_corner_variables()
		+ num_total_cuboid_axis_variables()
		+ num_total_reflection_symmetry_group_variables()
		+ num_total_rotation_symmetry_group_variables();
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_vector_variable(
	const std::pair<Index, Index>& _index_size_pair)
{
	assert(_index_size_pair.second > 0);
	NLPVectorExpression variable(_index_size_pair.second);
	for (unsigned int i = 0; i < _index_size_pair.second; ++i)
		variable[i] = NLPExpression(1.0, _index_size_pair.first + i);
	return variable;
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_cuboid_corner_variable_index_size(
	unsigned int _cuboid_index, unsigned int _corner_index) const
{
	const unsigned int dimension = 3;
	assert(_corner_index < MeshCuboid::k_num_corners);

	unsigned int index = cuboid_corner_variable_start_index_
		+ num_cuboid_corner_variables_ * _cuboid_index
		+ dimension * _corner_index;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_cuboid_axis_variable_index_size(
	unsigned int _cuboid_index, unsigned int _axis_index) const
{
	const unsigned int dimension = 3;
	assert(_axis_index < dimension);

	unsigned int index = cuboid_axis_variable_start_index_
		+ num_cuboid_axis_variables_ * _cuboid_index
		+ dimension * _axis_index;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_reflection_symmetry_group_variable_n_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_reflection_symmetry_groups_);

	unsigned int index = reflection_symmetry_group_variable_start_index_
		+ num_reflection_symmetry_group_variables_ * _symmetry_group_index
		+ 0;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_reflection_symmetry_group_variable_t_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_reflection_symmetry_groups_);

	unsigned int index = reflection_symmetry_group_variable_start_index_
		+ num_reflection_symmetry_group_variables_ * _symmetry_group_index
		+ dimension;
	return std::make_pair(index, 1);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_rotation_symmetry_group_variable_n_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_rotation_symmetry_groups_);

	unsigned int index = rotation_symmetry_group_variable_start_index_
		+ num_rotation_symmetry_group_variables_ * _symmetry_group_index
		+ 0;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_rotation_symmetry_group_variable_t_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_rotation_symmetry_groups_);

	unsigned int index = rotation_symmetry_group_variable_start_index_
		+ num_rotation_symmetry_group_variables_ * _symmetry_group_index
		+ dimension;
	return std::make_pair(index, dimension);
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_cuboid_corner_variable(
	unsigned int _cuboid_index, unsigned int _corner_index) const
{
	return create_vector_variable(get_cuboid_corner_variable_index_size(
		_cuboid_index, _corner_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_cuboid_axis_variable(
	unsigned int _cuboid_index, unsigned int _axis_index) const
{
	return create_vector_variable(get_cuboid_axis_variable_index_size(
		_cuboid_index, _axis_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_reflection_symmetry_group_variable_n(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_reflection_symmetry_group_variable_n_index_size(
		_symmetry_group_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_reflection_symmetry_group_variable_t(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_reflection_symmetry_group_variable_t_index_size(
		_symmetry_group_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_rotation_symmetry_group_variable_n(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_rotation_symmetry_group_variable_n_index_size(
		_symmetry_group_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_rotation_symmetry_group_variable_t(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_rotation_symmetry_group_variable_t_index_size(
		_symmetry_group_index));
}

void MeshCuboidNonLinearSolver::create_energy_functions(
	const Eigen::MatrixXd &_cuboid_quadratic_term,
	const Eigen::VectorXd &_cuboid_linear_term,
	const double _cuboid_constant_term,
	std::vector<NLPFunction *> &_functions)
{
	if (_cuboid_quadratic_term.rows() != num_total_cuboid_corner_variables()
		|| _cuboid_quadratic_term.cols() != num_total_cuboid_corner_variables()
		|| _cuboid_linear_term.rows() != num_total_cuboid_corner_variables())
	{
		std::cerr << "Error: " << std::endl;
		assert(false);
	}

	_functions.clear();

	Eigen::MatrixXd quadratic_term = Eigen::MatrixXd::Zero(num_total_variables(), num_total_variables());
	Eigen::VectorXd linear_term = Eigen::VectorXd::Zero(num_total_variables());
	double constant_term = _cuboid_constant_term;

	quadratic_term.block(0, 0,
		num_total_cuboid_corner_variables(), num_total_cuboid_corner_variables()) = _cuboid_quadratic_term;
	linear_term.segment(0, num_total_cuboid_corner_variables()) = _cuboid_linear_term;

	NLPFunction *function_1 = new NLPEigenQuadFunction(quadratic_term, linear_term, constant_term);
	_functions.push_back(function_1);

	//
	NLPFunction *fuction_2 = create_reflection_symmetry_group_energy_function();
	assert(fuction_2);
	_functions.push_back(fuction_2);

	NLPFunction *fuction_3 = create_rotation_symmetry_group_energy_function();
	assert(fuction_3);
	_functions.push_back(fuction_3);
	//
}

NLPFunction *MeshCuboidNonLinearSolver::create_reflection_symmetry_group_energy_function()
{
	Eigen::MatrixXd quadratic_term = Eigen::MatrixXd::Zero(num_total_variables(), num_total_variables());
	Eigen::VectorXd linear_term = Eigen::VectorXd::Zero(num_total_variables());
	double constant_term = 0;

	std::vector<ANNpointArray> cuboid_ann_points;
	std::vector<ANNkd_tree *> cuboid_ann_kd_tree;

	create_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		create_reflection_symmetry_group_energy_function(symmetry_group_index,
			cuboid_ann_points, cuboid_ann_kd_tree,
			quadratic_term, linear_term, constant_term);
	}

	delete_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	NLPFunction *function = new NLPEigenQuadFunction(quadratic_term, linear_term, constant_term);
	return function;
}

void MeshCuboidNonLinearSolver::create_reflection_symmetry_group_energy_function(
	const unsigned int _symmetry_group_index,
	const std::vector<ANNpointArray>& _cuboid_ann_points,
	const std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree,
	Eigen::MatrixXd& _quadratic_term,
	Eigen::VectorXd& _linear_term,
	double &_constant_term)
{
	const MeshCuboidSymmetryGroup* symmetry_group = reflection_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	std::list<MeshCuboidSymmetryGroup::WeightedPointPair> sample_point_pairs;
	symmetry_group->get_symmetric_sample_point_pairs(cuboids_,
		_cuboid_ann_points, _cuboid_ann_kd_tree, squared_neighbor_distance_,
		sample_point_pairs);

	if (sample_point_pairs.size() < min_num_symmetric_point_pairs_)
		return;

	// Eq (1):
	// min {(I - nn^T)(x - y)}^2. Let d = (x - y).
	// Since (I - nn^T)^2 = (I - nn^T),
	// => min d^T(I - nn^T)d = d^Td - d^Tnn^Td = d^Td - n^Tdd^Tn.

	// Eq (2):
	// min {n^T(x + y) - 2t}^2.

	Eigen::Matrix3d A1 = Eigen::Matrix3d::Zero();
	Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero(3 + 1, 3 + 1);
	Real c = 0;

	Real sum_weight = 0.0;
	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		assert((*it).weight_ >= 0);
		sum_weight += (*it).weight_;
	}

	if (sum_weight == 0)
		return;

	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		Real weight = (*it).weight_ / sum_weight;

		Eigen::Vector3d sum_p, diff_p;
		for (int i = 0; i < 3; ++i)
		{
			sum_p[i] = (*it).p1_[i] + (*it).p2_[i];
			diff_p[i] = (*it).p1_[i] - (*it).p2_[i];
		}

		A1 += (weight * (-1) * diff_p * diff_p.transpose());
		c += (weight * diff_p.transpose() * diff_p);

		Eigen::VectorXd b(3 + 1);
		b << sum_p, -2;
		A2 += (weight * b * b.transpose());
	}

	std::pair<Index, Index> index_size_pair;
	index_size_pair = get_reflection_symmetry_group_variable_n_index_size(_symmetry_group_index);

	_quadratic_term.block<3, 3>(index_size_pair.first, index_size_pair.first) +=
		symmetry_energy_term_weight_ * A1;

	// NOTE:
	// Variables 'n' and 't' are adjacent in the variable list.
	_quadratic_term.block<3 + 1, 3 + 1>(index_size_pair.first, index_size_pair.first) +=
		symmetry_energy_term_weight_ * A2;

	_constant_term += symmetry_energy_term_weight_ * c;
}

NLPFunction *MeshCuboidNonLinearSolver::create_rotation_symmetry_group_energy_function()
{
	NLPExpression expression;

	std::vector<ANNpointArray> cuboid_ann_points;
	std::vector<ANNkd_tree *> cuboid_ann_kd_tree;

	create_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		// Minimize \| t \|_2^2.
		NLPVectorExpression t_variable = create_rotation_symmetry_group_variable_t(symmetry_group_index);
		expression += NLPVectorExpression::dot_product(t_variable, t_variable);

		create_rotation_symmetry_group_energy_function(symmetry_group_index,
			cuboid_ann_points, cuboid_ann_kd_tree, expression);
	}

	delete_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	expression *= symmetry_energy_term_weight_;

	NLPFunction *function = new NLPSparseFunction(num_total_variables(), expression);
	return function;
}

void MeshCuboidNonLinearSolver::create_rotation_symmetry_group_energy_function(
	const unsigned int _symmetry_group_index,
	const std::vector<ANNpointArray>& _cuboid_ann_points,
	const std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree,
	NLPExpression &_expression)
{
	const MeshCuboidRotationSymmetryGroup* symmetry_group = rotation_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	NLPVectorExpression n_variable = create_rotation_symmetry_group_variable_n(_symmetry_group_index);
	NLPVectorExpression t_variable = create_rotation_symmetry_group_variable_t(_symmetry_group_index);

	std::list<MeshCuboidSymmetryGroup::WeightedPointPair> sample_point_pairs;
	symmetry_group->get_symmetric_sample_point_pairs(cuboids_,
		_cuboid_ann_points, _cuboid_ann_kd_tree, squared_neighbor_distance_,
		sample_point_pairs);

	if (sample_point_pairs.size() < min_num_symmetric_point_pairs_)
		return;

	Real sum_weight = 0.0;
	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		assert((*it).weight_ >= 0);
		sum_weight += (*it).weight_;
	}

	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		Real weight = (*it).weight_ / sum_weight;

		MyMesh::Normal n; MyMesh::Point t;
		symmetry_group->get_rotation_axis(n, t);

		int symmetry_order = static_cast<int>(std::round((*it).angle_ / symmetry_group->get_rotation_angle()));
		MyMesh::Point symmetry_point_1 = symmetry_group->get_symmetric_point((*it).p1_, symmetry_order);

		Eigen::Vector3d x_vec, y_vec;
		for (int i = 0; i < 3; ++i)
		{
			x_vec[i] = (*it).p1_[i];
			y_vec[i] = (*it).p2_[i];
		}

		// Debug.
		//Eigen::Vector3d n_vec, t_vec, symm_x_vec;
		//for (int i = 0; i < 3; ++i)
		//{
		//	n_vec[i] = n[i];
		//	t_vec[i] = t[i];
		//	symm_x_vec[i] = symmetry_point_1[i];
		//}

		//Eigen::AngleAxisd axis_rotation((*it).angle_, n_vec);
		//Eigen::VectorXd transf_x_vec = axis_rotation.toRotationMatrix() * (x_vec - t_vec) + t_vec;

		//Eigen::VectorXd diff_1 = transf_x_vec - symm_x_vec;
		//if (diff_1.norm() > 1.0E-8)
		//{
		//	std::cout << "distance = " << diff_1.norm() << std::endl;
		//	std::cout << "angle = " << (*it).angle_ << std::endl;
		//	std::cout << "symmetry_order = " << symmetry_order << std::endl;
		//	std::cout << symm_x_vec.transpose() << std::endl;
		//	std::cout << transf_x_vec.transpose() << std::endl;
		//	system("pause");
		//}

		//Eigen::VectorXd diff_2 = transf_x_vec - y_vec;
		//if (diff_2.norm() > neighbor_distance_)
		//{
		//	std::cout << "distance = " << diff_2.norm() << std::endl;
		//	std::cout << "neighbor_distance = " << neighbor_distance_ << std::endl;
		//	std::cout << "weight = " << (neighbor_distance_ - sqrt((*it).weight_)) << std::endl;
		//	std::cout << symm_x_vec.transpose() << std::endl;
		//	std::cout << transf_x_vec.transpose() << std::endl;
		//	system("pause");
		//}

		Real sin_angle = std::sin((*it).angle_);
		Real cos_angle = std::cos((*it).angle_);

		NLPVectorExpression x_minus_t = (x_vec - t_variable);
		NLPVectorExpression y_minus_t = (y_vec - t_variable);

		NLPVectorExpression term_1 = x_minus_t * cos_angle;
		NLPVectorExpression term_2 = NLPVectorExpression::cross_product(n_variable, x_minus_t) * sin_angle;
		NLPVectorExpression term_3 = n_variable * NLPVectorExpression::dot_product(n_variable, x_minus_t) * (1 - cos_angle);
		NLPVectorExpression all_terms = term_1 + term_2 + term_3 - y_minus_t;
		NLPExpression squared_all_terms = NLPVectorExpression::dot_product(all_terms, all_terms);

		// Debug.
		//Number *x = new Number[num_total_variables()];
		//std::pair<Index, Index> n_index_size = get_rotation_symmetry_group_variable_n_index_size(_symmetry_group_index);
		//std::pair<Index, Index> t_index_size = get_rotation_symmetry_group_variable_t_index_size(_symmetry_group_index);
		//for (Index i = 0; i < n_index_size.second; ++i)
		//	x[n_index_size.first + i] = n[i];
		//for (Index i = 0; i < t_index_size.second; ++i)
		//	x[t_index_size.first + i] = t[i];
		//std::cout << "eval = " << squared_all_terms.eval(x) << std::endl;

		//Eigen::Vector3d term_1_vec = cos_angle * (x_vec - t_vec);
		//Eigen::Vector3d term_2_vec = sin_angle * n_vec.cross(x_vec - t_vec);
		//Eigen::Vector3d term_3_vec = (1 - cos_angle) * n_vec.dot(x_vec - t_vec) * n_vec;
		//Eigen::Vector3d term_4_vec = -(y_vec - t_vec);
		//Eigen::Vector3d all_terms_vec = term_1_vec + term_2_vec + term_3_vec + term_4_vec;
		//std::cout << "eval = " << all_terms_vec.squaredNorm() << std::endl;

		//delete[] x;
		//system("pause");
		//

		_expression += squared_all_terms;
	}
}

void MeshCuboidNonLinearSolver::add_constraints(NLPFormulation &_formulation)
{
	add_cuboid_constraints(_formulation);
	add_reflection_symmetry_group_constraints(_formulation);
	add_rotation_symmetry_group_constraints(_formulation);
}

void MeshCuboidNonLinearSolver::add_cuboid_constraints(NLPFormulation &_formulation)
{
	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		add_cuboid_constraints(cuboid_index, _formulation);
	}
}

void MeshCuboidNonLinearSolver::add_reflection_symmetry_group_constraints(NLPFormulation &_formulation)
{
	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		add_reflection_symmetry_group_constraints(symmetry_group_index, _formulation);
	}
}

void MeshCuboidNonLinearSolver::add_rotation_symmetry_group_constraints(NLPFormulation &_formulation)
{
	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		add_rotation_symmetry_group_constraints(symmetry_group_index, _formulation);
	}
}

void MeshCuboidNonLinearSolver::add_cuboid_constraints(
	const unsigned int _cuboid_index,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = 3;

	for (unsigned int axis_index = 0; axis_index < dimension; ++axis_index)
	{
		NLPVectorExpression axis_variable_1 = create_cuboid_axis_variable(_cuboid_index, axis_index);

		// Unit vector constraint.
		_formulation.add_constraint(NLPVectorExpression::dot_product(
			axis_variable_1, axis_variable_1), 1, 1);

		// Orthogonality constraint.
		if (dimension >= 2)
		{
			NLPVectorExpression axis_variable_2 = create_cuboid_axis_variable(_cuboid_index,
				(axis_index + 1) % dimension);
			assert(axis_variable_1.dimension() == axis_variable_2.dimension());

			// NOTE:
			// Relaxing equality constraint.
			// The equality constraints cause "too few degrees" errors.
			_formulation.add_constraint(NLPVectorExpression::dot_product(
				axis_variable_1, axis_variable_2), -1.0E-12, 1.0E-12);
		}
	}

	for (int axis_index = 0; axis_index < dimension; ++axis_index)
	{
		// NOTE:
		// Implemented only for 3 dimension.
		assert(dimension == 3);

		for (int axis_1 = 0; axis_1 < 2; ++axis_1)
		{
			for (int axis_2 = 0; axis_2 < 2; ++axis_2)
			{
				std::bitset<3> bits;
				bits[(axis_index + 1) % 3] = axis_1;
				bits[(axis_index + 2) % 3] = axis_2;

				bits[axis_index] = false;
				int corner_index_1 = bits.to_ulong();

				bits[axis_index] = true;
				int corner_index_2 = bits.to_ulong();

				NLPVectorExpression corner_variable_1 = create_cuboid_corner_variable(_cuboid_index, corner_index_1);
				NLPVectorExpression corner_variable_2 = create_cuboid_corner_variable(_cuboid_index, corner_index_2);

				// A cuboid edge is orthogonal with two axes.
				for (int other_axis_index = 0; other_axis_index < dimension; ++other_axis_index)
				{
					if (other_axis_index == axis_index) continue;
					NLPVectorExpression other_axis_variable = create_cuboid_axis_variable(_cuboid_index, other_axis_index);

					// n^T(x - y) = 0.
					NLPExpression expression = NLPVectorExpression::dot_product(other_axis_variable,
						corner_variable_1 - corner_variable_2);

					// NOTE:
					// Relaxing equality constraint.
					// The equality constraints cause "too few degrees" errors.
					_formulation.add_constraint(expression, -1.0E-12, 1.0E-12);
				}
			}
		}
	}
}

void MeshCuboidNonLinearSolver::add_reflection_symmetry_group_constraints(
	const unsigned int _symmetry_group_index,
	NLPFormulation &_formulation)
{
	const MeshCuboidReflectionSymmetryGroup *symmetry_group = reflection_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	NLPVectorExpression n_variable = create_reflection_symmetry_group_variable_n(_symmetry_group_index);

	std::vector< unsigned int > single_cuboid_indices;
	symmetry_group->get_single_cuboid_indices(cuboids_, single_cuboid_indices);

	for (std::vector< unsigned int >::const_iterator it = single_cuboid_indices.begin();
		it != single_cuboid_indices.end(); ++it)
	{
		unsigned int cuboid_index = (*it);

		add_single_cuboid_reflection_constraints(
			cuboid_index,
			_symmetry_group_index,
			symmetry_group->get_aligned_global_axis_index(),
			_formulation);
	}

	std::vector< std::pair<unsigned int, unsigned int> > pair_cuboid_indices;
	symmetry_group->get_pair_cuboid_indices(cuboids_, pair_cuboid_indices);

	for (std::vector< std::pair<unsigned int, unsigned int> >::const_iterator it = pair_cuboid_indices.begin();
		it != pair_cuboid_indices.end(); ++it)
	{
		add_pair_cuboid_reflection_constraints(
			(*it).first, (*it).second,
			_symmetry_group_index,
			symmetry_group->get_aligned_global_axis_index(),
			_formulation);
	}

	// Unit vector constraint.
	NLPExpression expression = NLPVectorExpression::dot_product(
		n_variable, n_variable);
	_formulation.add_constraint(expression, 1, 1);
}

void MeshCuboidNonLinearSolver::add_reflection_constraints(
	const NLPVectorExpression& _x_variable,
	const NLPVectorExpression& _y_variable,
	const NLPVectorExpression& _n_variable,
	const NLPVectorExpression& _t_variable,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = _x_variable.dimension();
	assert(_y_variable.dimension() == dimension);
	assert(_n_variable.dimension() == dimension);
	assert(_t_variable.dimension() == 1);

	// n^T(x + y) - 2t = 0.
	NLPExpression expression_1 = NLPVectorExpression::dot_product(_n_variable,
		_x_variable + _y_variable)
		+ (_t_variable[0] * -2);

	// NOTE:
	// Relaxing equality constraint.
	// The equality constraints cause "too few degrees" errors.
	_formulation.add_constraint(expression_1, -1.0E-12, 1.0E-12);

	// n^T(x - y).
	NLPExpression temp = NLPVectorExpression::dot_product(_n_variable,
		_x_variable - _y_variable);

	// (I - nn^T)(x - y) = (x - y) - n(n^T(x - y)) = 0.
	NLPVectorExpression expression_2 = (_x_variable - _y_variable) - (_n_variable * temp);

	// NOTE:
	// Relaxing equality constraint.
	// The equality constraints cause "too few degrees" errors.
	_formulation.add_constraint(expression_2, -1.0E-12, 1.0E-12);
}

void MeshCuboidNonLinearSolver::add_single_cuboid_reflection_constraints(
	const unsigned int _cuboid_index,
	const unsigned int _symmetry_group_index,
	const unsigned int _reflection_axis_index,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = 3;
	assert(_reflection_axis_index < dimension);

	bool *is_corner_visited = new bool[MeshCuboid::k_num_corners];
	memset(is_corner_visited, false, MeshCuboid::k_num_corners * sizeof(bool));

	for (unsigned int corner_index_1 = 0; corner_index_1 < MeshCuboid::k_num_corners; ++corner_index_1)
	{
		if (is_corner_visited[corner_index_1])
			continue;

		std::bitset<3> bits(corner_index_1);
		bits[_reflection_axis_index].flip();
		unsigned int corner_index_2 = bits.to_ulong();

		assert(!is_corner_visited[corner_index_2]);

		add_reflection_constraints(
			create_cuboid_corner_variable(_cuboid_index, corner_index_1),
			create_cuboid_corner_variable(_cuboid_index, corner_index_2),
			create_reflection_symmetry_group_variable_n(_symmetry_group_index),
			create_reflection_symmetry_group_variable_t(_symmetry_group_index),
			_formulation);

		is_corner_visited[corner_index_1] = is_corner_visited[corner_index_2] = true;
	}

	delete[] is_corner_visited;
}

void MeshCuboidNonLinearSolver::add_pair_cuboid_reflection_constraints(
	const unsigned int _cuboid_index_1,
	const unsigned int _cuboid_index_2,
	const unsigned int _symmetry_group_index,
	const unsigned int _reflection_axis_index,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = 3;
	assert(_reflection_axis_index < dimension);

	for (unsigned int corner_index_1 = 0; corner_index_1 < MeshCuboid::k_num_corners; ++corner_index_1)
	{
		std::bitset<3> bits(corner_index_1);
		bits[_reflection_axis_index].flip();
		unsigned int corner_index_2 = bits.to_ulong();

		add_reflection_constraints(
			create_cuboid_corner_variable(_cuboid_index_1, corner_index_1),
			create_cuboid_corner_variable(_cuboid_index_2, corner_index_2),
			create_reflection_symmetry_group_variable_n(_symmetry_group_index),
			create_reflection_symmetry_group_variable_t(_symmetry_group_index),
			_formulation);
	}
}

void MeshCuboidNonLinearSolver::add_rotation_symmetry_group_constraints(
	const unsigned int _symmetry_group_index,
	NLPFormulation &_formulation)
{
	const MeshCuboidRotationSymmetryGroup *symmetry_group = rotation_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	const unsigned int dimension = 3;
	NLPVectorExpression n_variable = create_rotation_symmetry_group_variable_n(_symmetry_group_index);
	NLPVectorExpression t_variable = create_rotation_symmetry_group_variable_t(_symmetry_group_index);

	std::vector< unsigned int > single_cuboid_indices;
	symmetry_group->get_single_cuboid_indices(cuboids_, single_cuboid_indices);

	for (std::vector< unsigned int >::const_iterator it = single_cuboid_indices.begin();
		it != single_cuboid_indices.end(); ++it)
	{
		unsigned int cuboid_index = (*it);

		NLPVectorExpression axis_variable = create_cuboid_axis_variable(cuboid_index,
			symmetry_group->get_aligned_global_axis_index());

		// The symmetry axis should be the same direction with the cuboid axis.
		NLPVectorExpression expression_1 = n_variable - axis_variable;
		_formulation.add_constraint(NLPVectorExpression::dot_product(expression_1, expression_1), 0, 0);

		NLPVectorExpression cuboid_center_variable(dimension);
		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			cuboid_center_variable += create_cuboid_corner_variable(cuboid_index, corner_index);
		cuboid_center_variable *= (1.0 / MeshCuboid::k_num_corners);

		//  cross(n, (c - t)) = 0.
		NLPVectorExpression expression_2 = NLPVectorExpression::cross_product(
			n_variable, cuboid_center_variable - t_variable);
		_formulation.add_constraint(NLPVectorExpression::dot_product(expression_2, expression_2), 0, 0);
	}

	// Ignore pairs of cuboids.

	// Unit vector constraint.
	NLPExpression expression = NLPVectorExpression::dot_product(
		n_variable, n_variable);
	_formulation.add_constraint(expression, 1, 1);
}

void MeshCuboidNonLinearSolver::fix_cuboid(
	const unsigned int _cuboid_index,
	NLPFormulation &_formulation)
{
	const MeshCuboid* cuboid = cuboids_[_cuboid_index];
	assert(cuboid);

	const unsigned int dimension = 3;

	for (unsigned int corner_index = 0; corner_index < dimension; ++corner_index)
	{
		NLPVectorExpression axis_variable = create_cuboid_axis_variable(_cuboid_index, corner_index);
		MyMesh::Normal axis = cuboid->get_bbox_axis(corner_index);
		Eigen::Vector3d axis_vec;
		for (unsigned int i = 0; i < 3; ++i)
			axis_vec[i] = axis[i];

		axis_variable -= axis_vec;
		_formulation.add_constraint(NLPVectorExpression::dot_product(axis_variable, axis_variable), 0, 0);
	}

	for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
	{
		NLPVectorExpression corner_variable = create_cuboid_corner_variable(_cuboid_index, corner_index);
		MyMesh::Normal corner = cuboid->get_bbox_corner(corner_index);
		Eigen::Vector3d corner_vec;
		for (unsigned int i = 0; i < 3; ++i)
			corner_vec[i] = corner[i];

		corner_variable -= corner_vec;
		_formulation.add_constraint(NLPVectorExpression::dot_product(corner_variable, corner_variable), 0, 0);
	}
}

bool MeshCuboidNonLinearSolver::compute_initial_values(const Eigen::VectorXd &_input,
	Eigen::VectorXd &_output)
{
	if (_input.rows() != num_total_cuboid_corner_variables())
	{
		std::cerr << "Error: " << std::endl;
		return false;
	}

	_output = Eigen::VectorXd::Zero(num_total_variables());
	_output.segment(0, num_total_cuboid_corner_variables()) = _input;

	compute_cuboid_axis_values(_output);
	compute_reflection_symmetry_group_values(_output);
	compute_rotation_symmetry_group_values(_output);

	// DEBUG.
	for (int i = 0; i < num_total_variables(); ++i)
	{
		if (isnan(_output[i]) || isinf(_output[i]))
		{
			assert(false);
		}
	}

	return true;
}

void MeshCuboidNonLinearSolver::compute_cuboid_axis_values(Eigen::VectorXd &_values)
{
	assert(_values.rows() >= num_total_variables());

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		const MeshCuboid* cuboid = cuboids_[cuboid_index];
		assert(cuboid);
		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		{
			const MyMesh::Normal &axis = cuboid->get_bbox_axis(axis_index);

			std::pair<Index, Index> index_size_pair = get_cuboid_axis_variable_index_size(cuboid_index, axis_index);
			assert(index_size_pair.second == 3);
			for (unsigned int i = 0; i < index_size_pair.second; ++i)
				_values[index_size_pair.first + i] = axis[i];
		}
	}
}

void MeshCuboidNonLinearSolver::compute_reflection_symmetry_group_values(Eigen::VectorXd &_values)
{
	assert(_values.rows() >= num_total_variables());

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		const MeshCuboidReflectionSymmetryGroup* symmetry_group = reflection_symmetry_groups_[symmetry_group_index];
		assert(symmetry_group);

		MyMesh::Normal n; double t;
		symmetry_group->get_reflection_plane(n, t);

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_reflection_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == 3);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			_values[index_size_pair.first + i] = n[i];

		index_size_pair = get_reflection_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == 1);
		_values[index_size_pair.first] = t;
	}
}

void MeshCuboidNonLinearSolver::compute_rotation_symmetry_group_values(Eigen::VectorXd &_values)
{
	assert(_values.rows() >= num_total_variables());

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		const MeshCuboidRotationSymmetryGroup* symmetry_group = rotation_symmetry_groups_[symmetry_group_index];
		assert(symmetry_group);

		MyMesh::Normal n; MyMesh::Point t;
		symmetry_group->get_rotation_axis(n, t);

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_rotation_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == 3);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			_values[index_size_pair.first + i] = n[i];

		index_size_pair = get_rotation_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == 3);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			_values[index_size_pair.first + i] = t[i];
	}
}

void MeshCuboidNonLinearSolver::optimize(
	const Eigen::MatrixXd& _cuboid_quadratic_term,
	const Eigen::VectorXd& _cuboid_linear_term,
	const double _cuboid_constant_term,
	Eigen::VectorXd* _init_values_vec,
	const std::vector<unsigned int> *_fixed_cuboid_indices)
{
	// Update rotation angle.
	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		MeshCuboidRotationSymmetryGroup* symmetry_group = rotation_symmetry_groups_[symmetry_group_index];
		assert(symmetry_group);
		symmetry_group->compute_rotation_angle(cuboids_);
	}

	std::vector<NLPFunction *> functions;
	create_energy_functions(_cuboid_quadratic_term, _cuboid_linear_term, _cuboid_constant_term, functions);
	NLPFormulation formulation(functions);
	add_constraints(formulation);

	if (_fixed_cuboid_indices)
	{
		for (std::vector<unsigned int>::const_iterator it = (*_fixed_cuboid_indices).begin();
			it != (*_fixed_cuboid_indices).end(); ++it)
		{
			assert((*it) < cuboids_.size());
			fix_cuboid((*it), formulation);
		}
	}

	if (_init_values_vec)
	{
		Eigen::VectorXd all_init_values_vec;
		bool ret = compute_initial_values(*_init_values_vec, all_init_values_vec);
		assert(ret);
		formulation.set_values(all_init_values_vec.data());

		std::vector< Number > all_init_values(all_init_values_vec.rows());
		for (int i = 0; i < all_init_values_vec.rows(); ++i)
			all_init_values[i] = all_init_values_vec[i];

		std::cout << "Initial error = ";
		for (int i = 0; i < functions.size(); ++i)
			std::cout << functions[i]->eval(&all_init_values[0]) << ", ";
		std::cout << std::endl;
	}

	// ---- //
	// Create a new instance of your nlp
	//  (use a SmartPtr, not raw)
	SmartPtr<TNLP> mynlp = new IPOPTSolver(&formulation);
	//SmartPtr<TNLP> mynlp = new HS071_NLP();

	// Create a new instance of IpoptApplication
	//  (use a SmartPtr, not raw)
	// We are using the factory, since this allows us to compile this
	// example with an Ipopt Windows DLL
	SmartPtr<IpoptApplication> app = IpoptApplicationFactory();
	//app->RethrowNonIpoptException(true);

	// Change some options
	// Note: The following choices are only examples, they might not be
	//       suitable for your optimization problem.
	app->Options()->SetNumericValue("tol", 1e-8);
	app->Options()->SetStringValue("mu_strategy", "adaptive");
	app->Options()->SetIntegerValue("print_level", 0);
	app->Options()->SetStringValue("fixed_variable_treatment", "relax_bounds");
	// app->Options()->SetStringValue("output_file", "ipopt.out");
	// The following overwrites the default name (ipopt.opt) of the
	// options file
	// app->Options()->SetStringValue("option_file_name", "ipopt.opt");

	// Initialize the IpoptApplication and process the options
	ApplicationReturnStatus status;
	status = app->Initialize();
	if (status != Solve_Succeeded) {
		std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
		assert(false);
	}

	// Ask Ipopt to solve the problem
	status = app->OptimizeTNLP(mynlp);

	if (status == Solve_Succeeded) {
		//std::cout << std::endl << std::endl << "*** The problem solved!" << std::endl;
	}
	else {
		std::cout << std::endl << std::endl << "*** The problem FAILED!"
			<< " (" << static_cast<int>(status) << ")" << std::endl;
		//assert(false);
	}

	// As the SmartPtrs go out of scope, the reference count
	// will be decremented and the objects will automatically
	// be deleted.
	// ---- //

	// DEBUG.
	//formulation.print_constraint_evaluations();

	std::vector< Number > output;
	formulation.get_values(output);
	assert(output.size() == num_total_variables());
	//std::cout << "final = " << formulation.eval_function(&(output[0])) << ")" << std::endl;

	std::cout << "Final error = ";
	for (int i = 0; i < functions.size(); ++i)
		std::cout << functions[i]->eval(&output[0]) << ", ";
	std::cout << std::endl;

	// Update symmetry groups.
	update(output);
}

void MeshCuboidNonLinearSolver::update(const std::vector< Number >& _values)
{
	update_cuboids(_values);
	update_reflection_symmetry_groups(_values);
	update_rotation_symmetry_groups(_values);
}

void MeshCuboidNonLinearSolver::update_cuboids(const std::vector< Number >& _values)
{
	const unsigned int dimension = 3;

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		MeshCuboid *cuboid = cuboids_[cuboid_index];
		assert(cuboid);

		MyMesh::Point new_bbox_center(0.0);
		std::array<MyMesh::Point, MeshCuboid::k_num_corners> new_bbox_corners;
		std::array<MyMesh::Normal, dimension> new_bbox_axes;

		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
		{
			std::pair<Index, Index> index_size_pair;
			index_size_pair = get_cuboid_corner_variable_index_size(cuboid_index, corner_index);
			assert(index_size_pair.second == dimension);
			for (unsigned int i = 0; i < index_size_pair.second; ++i)
				new_bbox_corners[corner_index][i] = _values[index_size_pair.first + i];

			new_bbox_center += new_bbox_corners[corner_index];
		}
		new_bbox_center = new_bbox_center / MeshCuboid::k_num_corners;

		for (unsigned int axis_index = 0; axis_index < dimension; ++axis_index)
		{
			std::pair<Index, Index> index_size_pair;
			index_size_pair = get_cuboid_axis_variable_index_size(cuboid_index, axis_index);
			assert(index_size_pair.second == dimension);
			for (unsigned int i = 0; i < index_size_pair.second; ++i)
				new_bbox_axes[axis_index][i] = _values[index_size_pair.first + i];

			new_bbox_axes[axis_index].normalize();

			// Flip axes.
			MyMesh::Point minus_axis_direction(0.0), plus_axis_direction(0.0);

			for (int axis_1 = 0; axis_1 < 2; ++axis_1)
			{
				for (int axis_2 = 0; axis_2 < 2; ++axis_2)
				{
					std::bitset<3> bits;
					bits[(axis_index + 1) % 3] = axis_1;
					bits[(axis_index + 2) % 3] = axis_2;

					bits[axis_index] = false;
					int minus_corner_index = bits.to_ulong();
					minus_axis_direction += new_bbox_corners[minus_corner_index];

					bits[axis_index] = true;
					int plus_corner_index = bits.to_ulong();
					plus_axis_direction += new_bbox_corners[plus_corner_index];
				}
			}

			// Compare the axis with the direction from the center to the average of corner points on +/- side.
			minus_axis_direction = (minus_axis_direction / 4.0 - new_bbox_center).normalized();
			plus_axis_direction = (plus_axis_direction / 4.0 - new_bbox_center).normalized();

			//if (dot(new_bbox_axes[axis_index], minus_axis_direction)
			//	> dot(new_bbox_axes[axis_index], plus_axis_direction))
			if (dot(new_bbox_axes[axis_index], cuboid->get_bbox_axis(axis_index)) < 0)
			{
				// Flip.
				new_bbox_axes[axis_index] = -new_bbox_axes[axis_index];
			}
		}

		cuboid->set_bbox_center(new_bbox_center);
		cuboid->set_bbox_corners(new_bbox_corners);
		cuboid->set_bbox_axes(new_bbox_axes, false);

		// Update cuboid surface points.
		for (unsigned int point_index = 0; point_index < cuboid->num_cuboid_surface_points();
			++point_index)
		{
			MeshCuboidSurfacePoint *cuboid_surface_point = cuboid->get_cuboid_surface_point(point_index);

			MyMesh::Point new_point(0.0);
			for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
				new_point += cuboid_surface_point->corner_weights_[corner_index] * new_bbox_corners[corner_index];

			cuboid_surface_point->point_ = new_point;
		}
	}
}

void MeshCuboidNonLinearSolver::update_reflection_symmetry_groups(const std::vector< Number >& _values)
{
	const unsigned int dimension = 3;

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		MyMesh::Normal n; double t;

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_reflection_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == dimension);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			n[i] = _values[index_size_pair.first + i];

		index_size_pair = get_reflection_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == 1);
		t = _values[index_size_pair.first];

		reflection_symmetry_groups_[symmetry_group_index]->set_reflection_plane(n, t);
	}
}

void MeshCuboidNonLinearSolver::update_rotation_symmetry_groups(const std::vector< Number >& _values)
{
	const unsigned int dimension = 3;

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		MyMesh::Normal n; MyMesh::Point t;

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_rotation_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == dimension);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			n[i] = _values[index_size_pair.first + i];

		index_size_pair = get_rotation_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == dimension);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			t[i] = _values[index_size_pair.first + i];

		rotation_symmetry_groups_[symmetry_group_index]->set_rotation_axis(n, t);
	}
}

void MeshCuboidNonLinearSolver::create_cuboid_sample_point_ann_trees(
	std::vector<ANNpointArray>& _cuboid_ann_points,
	std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree) const
{
	delete_cuboid_sample_point_ann_trees(_cuboid_ann_points, _cuboid_ann_kd_tree);
	_cuboid_ann_points.resize(num_cuboids_);
	_cuboid_ann_kd_tree.resize(num_cuboids_);

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		_cuboid_ann_kd_tree[cuboid_index] = NULL;
		_cuboid_ann_points[cuboid_index] = NULL;

		MeshCuboid *cuboid = cuboids_[cuboid_index];
		unsigned int num_cuboid_sample_points = cuboid->num_sample_points();
		if (num_cuboid_sample_points == 0)
			continue;

		Eigen::MatrixXd cuboid_sample_points(3, num_cuboid_sample_points);

		for (unsigned int point_index = 0; point_index < num_cuboid_sample_points; ++point_index)
		{
			for (unsigned int i = 0; i < 3; ++i)
				cuboid_sample_points.col(point_index)(i) =
				cuboid->get_sample_point(point_index)->point_[i];
		}

		_cuboid_ann_kd_tree[cuboid_index] = ICP::create_kd_tree(cuboid_sample_points,
			_cuboid_ann_points[cuboid_index]);
		assert(_cuboid_ann_points[cuboid_index]);
		assert(_cuboid_ann_kd_tree[cuboid_index]);
	}
}

void MeshCuboidNonLinearSolver::delete_cuboid_sample_point_ann_trees(
	std::vector<ANNpointArray>& _cuboid_ann_points,
	std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree) const
{
	if (_cuboid_ann_points.empty() && _cuboid_ann_kd_tree.empty())
		return;

	assert(_cuboid_ann_points.size() == num_cuboids_);
	assert(_cuboid_ann_kd_tree.size() == num_cuboids_);

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		if (_cuboid_ann_points[cuboid_index]) annDeallocPts(_cuboid_ann_points[cuboid_index]);
		if (_cuboid_ann_kd_tree[cuboid_index]) delete _cuboid_ann_kd_tree[cuboid_index];
	}

	_cuboid_ann_points.clear();
	_cuboid_ann_kd_tree.clear();
}


// [[Rcpp::plugins(openmp)]]
#ifdef _OPENMP
#include <omp.h>
#endif
#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Core>
#include <random>
#include "distributions.h"
#include "concurrentqueue.h"
// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using namespace RcppEigen;
using namespace Eigen;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::SparseVector;
using Eigen::LLT;
using Eigen::Lower;
using Eigen::Map;
using Eigen::Upper;
typedef Map<MatrixXd> MapMatd;

inline void initialize_file( std::ofstream& outFile,int M,int N,int groups, int F){
  bool queueFull;
  queueFull=0;

  outFile<< "iteration,"<<"mu,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "beta[" << (i+1) << "],";

  }
  outFile<<"sigmaE,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "comp[" << (i+1) << "],";
  }
  for(unsigned int i = 0; i < groups; ++i){
    outFile << "sigmaG[" << (i+1) << "],";
  }
  unsigned int i;
  for(i = 0; i < (N-1);i++){
    outFile << "epsilon[" << (i+1) << "],";
  }

  outFile << "epsilon[" << (i+1) << "],";

  for(i = 0; i < F; i ++){
    outFile << "alpha[" << (i+1) << "],";
  }
  outFile << "sigmaF";
  outFile<<"\n";
}

//' BayesRR sampler
//' @param outputFile The file in which the samples aftare burnin will be stored
//' @param seed random seed
//' @param max_iterations total of number of samples taken.
//' @param burn_in integer leq than max_iterations, number of samples used for burn in, after which, al samples will  be stored in the outputFile
//' @param thinning thinning regime, not implemented
//' @param X matrix of snp markers, or covariates of interest
//' @param Y vector of response variates, must have the same number of rows as X
//' @param sigma0 variance of the zero-centered normal prior over the intercept
//' @param v0E degrees of  freedom of the prior inverse scaled chi-squared distribution over residues variance
//' @param s02E scale parameter of the prior inverse scaled chi-squared distribution over residues variance
//' @param v0G degrees of freedom of the prior inverse scaled chi-squared distribution over genetic effects variance
//' @param s02G scale parameter of the prior inverse scaled chi-squared distribution over genetic effects variance
//' @param cva Matrix of mixture variances for groups, rows must be the same as number of groups, columns the same as number of mixtures.
//' @param groups number of groups
//' @param gAssign Vector of the same size as the number of columns of X, containing group assignments for each column(starting with group 0).
//' @param fixed Matrxi with same rows as X and Y , contains the fixed effects, those whose prior is always different than zero
// [[Rcpp::export]]
void BayesRSamplerV2Groups(std::string outputFile, int seed, int max_iterations, int burn_in, int thinning, Eigen::MatrixXd X, Eigen::VectorXd Y,double sigma0, double v0E, double s02E, double v0G, double s02G,Eigen::MatrixXd cva,int groups, Eigen::VectorXi gAssign, Eigen::MatrixXd fixed) {
  int flag;
  moodycamel::ConcurrentQueue<Eigen::VectorXd> q;
  flag=0;
  int N(Y.size());
  int M(X.cols());
  int F(fixed.cols());
  VectorXd components(M);
  std::ofstream outFile;

  outFile.open(outputFile);
  int K(cva.cols()+1);
  VectorXd sampleq(2*M+3+groups+ N + F+ 1);
  IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", "", "");
  ////////////validate inputs

  if(max_iterations < burn_in || max_iterations<1 || burn_in<1) //validations related to mcmc burnin and iterations
  {
    Rcpp::Rcerr<<"error: burn_in has to be a positive integer and smaller than the maximum number of iterations ";
    return;
  }
  if(sigma0 < 0 || v0E < 0 || s02E < 0 || v0G < 0||  s02G < 0 )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: hyper parameters have to be positive";
    //return;
  }
  if((cva.array()==0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the zero component is already included in the model by default";
    //return;
  }
  if((cva.array()<0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the variance of the components should be positive";
    //return;
  }
  /////end of declarations//////

  initialize_file(outFile,M,N,groups,F);
 // Eigen::initParallel();
//  Eigen::setNbThreads(10);
  //double sum_beta_sqr;
#ifdef _OPENMP
  omp_set_num_threads(2);
#endif
#pragma omp parallel num_threads(2) shared(flag,q,M,N,F)
{
#pragma omp sections
{
  //producer
  {

    //mean and residual variables
    double mu; // mean or intercept
    double sigmaG; //genetic variance
    double sigmaE; // residuals variance
    VectorXd sigmaGG(groups);
    double sigmaF; // fixed effects variance

    //component variables
    MatrixXd priorPi(groups,K); // prior probabilities for each component
    MatrixXd pi(groups,K); // mixture probabilities
    VectorXd cVa(K); //component-specific variance
    VectorXd logL(K); // log likelihood of component
    VectorXd muk(K); // mean of k-th component marker effect size
    VectorXd denom(K-1); // temporal variable for computing the inflation of the effect variance for a given non-zero componnet
    int m0; // total num ber of markes in model
    MatrixXd v(groups,K); //variable storing the component assignment
    VectorXd cVaI(K);// inverse of the component variances

    //linear model variables
    MatrixXd beta(M,1); // effect sizes
    VectorXd alpha(F); // fixed effects
    VectorXd y_tilde(N); // variable containing the adjusted residuals to exclude the effects of a given marker
    VectorXd epsilon(N); // variable containing the residuals

    //sampler variables
    VectorXd sample(2*M+3+groups+ N + F+ 1); // varible containg a sambple of all variables in the model, M marker effects, M component assigned to markers, sigmaE, sigmaG, mu, iteration number and Explained variance
    std::vector<int> markerI;
    for (int i=0; i<M; ++i) {
      markerI.push_back(i);
    }
    std::vector<int> fixedI;
    for(int j=0; j<F; ++j){
      fixedI.push_back(j);
    }
    VectorXd xsquared(M);
    double num;

    int marker;
    double acum;
    VectorXd betaAcum(groups);

        for(int i=0; i < groups; i++){
        	priorPi.row(i)(0)=0.5;
        	for(int k=1;k<K;k++){
        	priorPi.row(i)(k)=0.5/K;
        	}
        }
    y_tilde.setZero();
    //cVa[0] = 0;
    //cVa.segment(1,(K-1))=cva;

    //cVaI[0] = 0;
    //cVaI.segment(1,(K-1))=cVa.segment(1,(K-1)).cwiseInverse();
    //beta=beta.setRandom();

    //beta=(beta.array().abs() > 1e-6  ).select(beta, MatrixXd::Zero(M,1));
    beta.setZero();
    alpha.setZero();

    //mu=norm_rng(0,1);
    mu=0;

    betaAcum.setZero();
    // sigmaG=(1*cVa).sum()/M;
    for(int i=0; i<groups;i++)
      sigmaGG[i]=beta_rng(1,1);

    sigmaF= (double)R::runif(0,1);

    pi=priorPi;

    components.setZero();
    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    epsilon= Y.array() - mu;
    sigmaE=epsilon.squaredNorm()/N*0.5;
    xsquared=X.colwise().squaredNorm();
    for(int iteration=0; iteration < max_iterations; iteration++){

      if(iteration>0)
        if( iteration % (int)std::ceil(max_iterations/10) ==0)
          Rcpp::Rcout << "iteration: "<<iteration <<"\n";

        epsilon= epsilon.array()+mu;//  we substract previous value
        mu = norm_rng(epsilon.sum()/(double)N, sigmaE/(double)N); //update mu
        epsilon= epsilon.array()-mu;// we substract again now epsilon =Y-mu-X*beta

        std::random_shuffle(fixedI.begin(), fixedI.end());
        for(int cur_fix=0; cur_fix < F; cur_fix++){
          int cur = fixedI[cur_fix];
          auto cur_alpha = alpha[cur];
          y_tilde= epsilon.array()+(fixed.col(cur)*cur_alpha).array();
          auto denom_f = (N-1) + (sigmaE/sigmaF);
          auto num_f = (fixed.col(cur).cwiseProduct(y_tilde)).sum();
          alpha[cur] = norm_rng(num_f/denom_f,sigmaE/denom_f);
          epsilon = y_tilde - fixed.col(cur)*alpha[cur];
        }

        std::random_shuffle(markerI.begin(), markerI.end());

        m0=0;
        v.setZero();
        betaAcum.setZero();
        for(int j=0; j < M; j++){

          marker= markerI[j];
          sigmaG=sigmaGG[gAssign(marker)];

          cVa[0]=0;
          cVaI[0]=0;
          cVa.segment(1,(K-1))=cva.row(gAssign(marker));
          cVaI.segment(1,(K-1))=(cVa.segment(1,(K-1))).cwiseInverse();

          y_tilde= epsilon.array()+(X.col(marker)*beta(marker,0)).array();//now y_tilde= Y-mu-X*beta+ X.col(marker)*beta(marker)_old

          muk[0]=0.0;//muk for the zeroth component=0

          // Rcpp::Rcout<< muk;
          //we compute the denominator in the variance expression to save computations
          denom=xsquared(marker)+(sigmaE/sigmaG)*cVaI.segment(1,(K-1)).array();
          //muk for the other components is computed according to equaitons
          num=(X.col(marker).cwiseProduct(y_tilde)).sum();
          muk.segment(1,(K-1))= num/denom.array();

          logL= pi.row(gAssign(marker)).array().log();//first component probabilities remain unchanged

          // Here we reproduce the fortran code
          logL.segment(1,(K-1))=logL.segment(1,(K-1)).array() - 0.5*((((sigmaG/sigmaE)*(xsquared(marker))*cVa.segment(1,(K-1)).array() + 1).array().log()))+
            0.5*( muk.segment(1,(K-1)).array()*num)/sigmaE;

          double p((double)R::runif( 0.0, 1.0 ));//

          if(((logL.segment(1,(K-1)).array()-logL[0]).abs().array() >700 ).any() ){
            acum=0;
          }else{
            acum=1.0/((logL.array()-logL[0]).exp().sum());
          }

          for(int k=0;k<K;k++){
            if(p<=acum){
              if(k==0){
                beta(marker,0)=0;
              }else{
                beta(marker,0)=norm_rng(muk[k],sigmaE/denom[k-1]);
                betaAcum(gAssign(marker))+= pow(beta(marker,0),2);
                // beta(marker,0)=norm_rng(rhs/denom[k-1],sigmaE/denom[k-1]);
              }
              v.row(gAssign(marker))(k)+=1.0;
              components[marker]=k;
              break;
            }else{
              if(((logL.segment(1,(K-1)).array()-logL[k+1]).abs().array() >700 ).any() ){
                acum+=0;
              }
              else{
                acum+=1.0/((logL.array()-logL[k+1]).exp().sum());
              }
            }
          }
          epsilon=y_tilde-X.col(marker)*beta(marker,0);//now epsilon contains Y-mu - X*beta+ X.col(marker)*beta(marker)_old- X.col(marker)*beta(marker)_new

        }

        sigmaF = inv_scaled_chisq_rng(v0E+F,(alpha.squaredNorm()+v0E*s02E)/(v0E+F));
        // Rcpp::Rcout << sigmaF;

        sigmaE=inv_scaled_chisq_rng(v0E+N,((epsilon).squaredNorm()+v0E*s02E)/(v0E+N));

        for(int i=0; i<groups; i++){
          m0=v.row(i).sum()-v.row(i)(0);
          sigmaGG[i]=inv_scaled_chisq_rng(v0G+m0,(betaAcum(i)*m0+v0G*s02G)/(v0G+m0));
          pi.row(i)=dirichilet_rng(v.row(i).array() + 1.0);

        }

        if(iteration >= burn_in)
        {
          if(iteration % thinning == 0){
            sample<< iteration,mu,beta,sigmaE,components,sigmaGG,epsilon,alpha,sigmaF;

            q.enqueue(sample);
            //here we have the consumer
#ifdef _OPENMP

#else
            if(q.try_dequeue(sampleq)){

              outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
            }
#endif
          }

        }

    }

    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>( t2 - t1 ).count();
    Rcpp::Rcout << "duration: "<<duration << "s\n";
    flag=1;
  }//end producer
  //consumer
    #ifdef _OPENMP
    #pragma omp section
    {

      while(!flag ){
        if(q.try_dequeue(sampleq)){
          outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
        }
      }//while

    }// endconsumer
    #endif
}

}

}


// 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/>.

#ifndef SAMPLER_HH
#define SAMPLER_HH

#include "random.hh"
#include <functional>
#include <boost/mpl/if.hpp>

namespace graph_tool
{
using namespace std;
using namespace boost;

// Discrete sampling via vose's alias method.

// See http://www.keithschwarz.com/darts-dice-coins/ for a very clear
// explanation.

template <class Value, class KeepReference = mpl::true_>
class Sampler
{
public:
    Sampler(const vector<Value>& items,
            const vector<double>& probs)
        : _items(items), _probs(probs), _alias(items.size())
    {
        double S = 0;
        for (size_t i = 0; i < _probs.size(); ++i)
            S += _probs[i];

        for (size_t i = 0; i < _probs.size(); ++i)
        {
            _probs[i] *= _probs.size() / S;
            if (_probs[i] < 1)
                _small.push_back(i);
            else
                _large.push_back(i);
        }

        while (!(_small.empty() || _large.empty()))
        {
            size_t l = _small.back();
            size_t g = _large.back();
            _small.pop_back();
            _large.pop_back();

            _alias[l] = g;
            _probs[g] = (_probs[l] + _probs[g]) - 1;
            if (_probs[g] < 1)
                _small.push_back(g);
            else
                _large.push_back(g);
        }

        // fix numerical instability
        for (size_t i = 0; i < _large.size(); ++i)
            _probs[_large[i]] = 1;
        for (size_t i = 0; i < _small.size(); ++i)
            _probs[_small[i]] = 1;
        _large.clear();
        _small.clear();
    }

    Sampler() {}

    template <class RNG>
    const Value& sample(RNG& rng)
    {
        uniform_int_distribution<size_t> sample(0, _probs.size() - 1);
        size_t i = sample(rng);

        bernoulli_distribution coin(_probs[i]);
        if (coin(rng))
            return _items[i];
        else
            return _items[_alias[i]];
    }

    size_t size() const { return _items.size(); }

private:

    typedef typename mpl::if_<KeepReference,
                              const vector<Value>&,
                              vector<Value> >::type items_t;
    items_t _items;
    vector<double> _probs;
    vector<size_t> _alias;
    vector<size_t> _small;
    vector<size_t> _large;
};

// uniform sampling from containers

template <class Container, class RNG>
const typename Container::value_type& uniform_sample(const Container& v, RNG& rng)
{
    std::uniform_int_distribution<size_t> i_rand(0, v.size() - 1);
    return v[i_rand(rng)];
}

} // namespace graph_tool

#endif // SAMPLER_HH


// ====================================================================
// 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/>.
// ====================================================================

#ifndef MSSM_TWOLOOPHIGGS_H
#define MSSM_TWOLOOPHIGGS_H

#include <Eigen/Core>

/**
 * @file mssm_twoloophiggs.hpp
 * @brief function declarations for 2-loop MSSM Higgs self-energies
 *        and tadpoles
 *
 * Notation:
 *
 * mt2    : squared DR-bar top mass in the MSSM
 * mb2    : squared DR-bar bottom mass in the MSSM
 * mtau2  : squared DR-bar tau mass in the MSSM
 * mg     : DR-bar gluino mass in the MSSM
 * mA2    : squared DR-bar CP-odd Higgs mass in the MSSM
 * mst12  : squared DR-bar lightest stop mass
 * mst22  : squared DR-bar heaviest stop mass
 * msb12  : squared DR-bar lightest sbottom mass
 * msb22  : squared DR-bar heaviest sbottom mass
 * mstau12: squared DR-bar lightest stau mass
 * mstau22: squared DR-bar heaviest stau mass
 * msv2   : squared DR-bar tau sneutrino mass
 *
 * sxt    : sine of DR-bar stop mixing angle in the MSSM
 * cxt    : cosine of DR-bar stop mixing angle in the MSSM
 * sxb    : sine of DR-bar sbottom mixing angle in the MSSM
 * cxb    : cosine of DR-bar sbottom mixing angle in the MSSM
 * sintau : sine of DR-bar stau mixing angle in the MSSM
 * costau : cosine of DR-bar stau mixing angle in the MSSM
 *
 * gs     : DR-bar strong gauge coupling g3 in the MSSM
 * mu     : DR-bar mu-parameter in the MSSM (arXiv:0907.4682)
 * tanb   : DR-bar tan(beta) = vu/vd in the MSSM
 * cotb   : DR-bar 1/tan(beta) in the MSSM
 * vev2   : squared DR-bar vev^2 = (vu^2 + vd^2) in the MSSM
 *
 * scheme : DR-bar scheme (0) or on-shell scheme (1)
 */

namespace flexiblesusy {
namespace mssm_twoloophiggs {

// tadpoles

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_at_as_mssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2,
   double mu, double tanb, double vev2, double gs);

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_at_at_mssm(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_ab_as_mssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2,
   double mu, double cotb, double vev2, double gs);

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_atau_atau_mssm(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2);

// self-energies

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_as_mssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_at_mssm(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_ab_as_mssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_atau_atau_mssm(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_at_as_mssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_at_at_mssm(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_ab_as_mssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_atau_atau_mssm(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2);

// self-energies with tadpoles added

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_as_mssm_with_tadpoles(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_at_mssm_with_tadpoles(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_ab_as_mssm_with_tadpoles(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_atau_atau_mssm_with_tadpoles(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2, int scheme = 0);

double self_energy_pseudoscalar_2loop_at_as_mssm_with_tadpoles(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs);

double self_energy_pseudoscalar_2loop_at_at_mssm_with_tadpoles(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

double self_energy_pseudoscalar_2loop_ab_as_mssm_with_tadpoles(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs);

double self_energy_pseudoscalar_2loop_atau_atau_mssm_with_tadpoles(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2);

} // namespace mssm_twoloophiggs
} // namespace flexiblesusy

#endif